fix(cpu): improve MRS and MSR instructions

src/cpu.zig

@@ -201,9 +201,10 @@ fn armPopulate() [0x1000]ArmInstrFn {
             if (i >> 10 & 0x3 == 0b00 and i >> 7 & 0x3 == 0b10 and i >> 4 & 1 == 0) {
                 // PSR Transfer
                 const I = i >> 9 & 1 == 1;
-                const isSpsr = i >> 6 & 1 == 1;
+                const R = i >> 6 & 1 == 1;
+                const kind = i >> 4 & 0x3;
 
-                lut[i] = psrTransfer(I, isSpsr);
+                lut[i] = psrTransfer(I, R, kind);
             }
 
             if (i == 0x121) {

src/cpu/arm/barrel_shifter.zig

@@ -33,7 +33,7 @@ fn immShift(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() + 4 else cpu.r[rm_idx];
+    const rm = if (rm_idx == 0xF) cpu.fakePC() else cpu.r[rm_idx];
 
     var result: u32 = undefined;
     if (amount == 0) {

src/cpu/arm/data_processing.zig

@@ -8,7 +8,7 @@ const InstrFn = @import("../../cpu.zig").ArmInstrFn;
 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 = opcode >> 12 & 0xF;
+            const rd = @truncate(u4, opcode >> 12 & 0xF);
             const rn = opcode >> 16 & 0xF;
             const old_carry = @boolToInt(cpu.cpsr.c.read());
 
@@ -23,18 +23,30 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
             }
 
             switch (instrKind) {
-                0x2 => {
-                    // SUB
-                    const result = op1 -% op2;
+                0x0 => {
+                    // AND
+                    const result = op1 & op2;
                     cpu.r[rd] = result;
 
                     if (S and rd != 0xF) {
                         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);
+                        // C set by Barrel Shifter, V is unaffected
                     }
                 },
+                0x1 => {
+                    // EOR
+                    const result = op1 ^ op2;
+                    cpu.r[rd] = result;
+
+                    if (S and rd != 0xF) {
+                        cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                        cpu.cpsr.z.write(result == 0);
+                        // C set by Barrel Shifter, V is unaffected
+                    }
+                },
+                0x2 => sub(S, cpu, rd, op1, op2), // SUB
+                0x3 => sub(S, cpu, rd, op2, op1), // RSB
                 0x4 => {
                     // ADD
                     var result: u32 = undefined;
@@ -63,20 +75,8 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                         cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
                     }
                 },
-                0x6 => {
-                    // SBC
-                    // TODO: Make your own
-                    const subtrahend = @as(u64, op2) - old_carry + 1;
-                    const result = @truncate(u32, op1 -% subtrahend);
-                    cpu.r[rd] = result;
-
-                    if (S and rd != 0xF) {
-                        cpu.cpsr.n.write(result >> 31 & 1 == 1);
-                        cpu.cpsr.z.write(result == 0);
-                        cpu.cpsr.c.write(subtrahend <= op1);
-                        cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
-                    }
-                },
+                0x6 => sbc(S, cpu, rd, op1, op2, old_carry), // SBC
+                0x7 => sbc(S, cpu, rd, op2, op1, old_carry), // RSC
                 0x8 => {
                     // TST
                     const result = op1 & op2;
@@ -95,6 +95,36 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                     // Barrel Shifter should always calc CPSR C in TEQ
                     if (!S) _ = shifter.execute(true, cpu, opcode);
                 },
+                0xA => {
+                    // CMP
+                    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);
+                },
+                0xB => {
+                    // CMN
+                    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);
+                },
+                0xC => {
+                    // ORR
+                    const result = op1 | op2;
+                    cpu.r[rd] = result;
+
+                    if (S and rd != 0xF) {
+                        cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                        cpu.cpsr.z.write(result == 0);
+                        // C set by Barrel Shifter, V is unaffected
+                    }
+                },
                 0xD => {
                     // MOV
                     cpu.r[rd] = op2;
@@ -105,18 +135,9 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                         // C set by Barrel Shifter, V is unaffected
                     }
                 },
-                0xA => {
-                    // CMP
-                    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);
-                },
-                0xC => {
-                    // ORR
-                    const result = op1 | op2;
+                0xE => {
+                    // BIC
+                    const result = op1 & ~op2;
                     cpu.r[rd] = result;
 
                     if (S and rd != 0xF) {
@@ -136,8 +157,33 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                         // C set by Barrel Shifter, V is unaffected
                     }
                 },
-                else => std.debug.panic("[CPU] TODO: implement data processing type {}", .{instrKind}),
             }
         }
     }.inner;
 }
+
+fn sbc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32, old_carry: u1) void {
+    // TODO: Make your own version (thanks peach.bot)
+    const subtrahend = @as(u64, right) - old_carry + 1;
+    const result = @truncate(u32, left -% subtrahend);
+    cpu.r[rd] = result;
+
+    if (S and rd != 0xF) {
+        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);
+    }
+}
+
+fn sub(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) void {
+    const result = left -% right;
+    cpu.r[rd] = result;
+
+    if (S and rd != 0xF) {
+        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);
+    }
+}

src/cpu/arm/psr_transfer.zig

@@ -3,49 +3,59 @@ const std = @import("std");
 const Bus = @import("../../Bus.zig");
 const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
 const InstrFn = @import("../../cpu.zig").ArmInstrFn;
+const PSR = @import("../../cpu.zig").PSR;
 
-pub fn psrTransfer(comptime I: bool, comptime isSpsr: bool) InstrFn {
+pub fn psrTransfer(comptime I: bool, comptime R: bool, comptime kind: u2) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
-            switch (@truncate(u3, opcode >> 19)) {
-                0b001 => {
+            switch (kind) {
+                0b00 => {
                     // MRS
-                    const rn = opcode >> 12 & 0xF;
+                    const rd = opcode >> 12 & 0xF;
 
-                    if (isSpsr) {
-                        std.debug.panic("[CPU] TODO: MRS on SPSR_<current_mode> is unimplemented", .{});
+                    if (R) {
+                        std.debug.panic("[CPU/PSR Transfer] TODO: MRS on SPSR_<current_mode> is unimplemented", .{});
                     } else {
-                        cpu.r[rn] = cpu.cpsr.raw;
+                        cpu.r[rd] = cpu.cpsr.raw;
                     }
                 },
-                0b101 => {
+                0b10 => {
                     // MSR
-                    const rm = opcode & 0xF;
+                    const field_mask = @truncate(u4, opcode >> 16 & 0xF);
 
-                    switch (@truncate(u3, opcode >> 16)) {
-                        0b000 => {
-                            const right = if (I) std.math.rotr(u32, opcode & 0xFF, opcode >> 7 & 0xF) else cpu.r[rm];
+                    if (I) {
+                        const imm = std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1);
 
-                            if (isSpsr) {
-                                std.debug.panic("[CPU] TODO: MSR (flags only) on SPSR_<current_mode> is unimplemented", .{});
+                        if (R) {
+                            std.debug.panic("[CPU/PSR Transfer] TODO: MSR (flags only) on SPSR_<current_mode> is unimplemented", .{});
                         } else {
-                                const mask: u32 = 0xF000_0000;
-                                cpu.cpsr.raw = (cpu.cpsr.raw & ~mask) | (right & mask);
+                            cpu.cpsr.raw = fieldMask(&cpu.cpsr, field_mask, imm);
                         }
-                        },
-                        0b001 => {
-                            if (isSpsr) {
-                                std.debug.panic("[CPU] TODO: MSR on SPSR_<current_mode> is unimplemented", .{});
                     } else {
-                                cpu.cpsr = .{ .raw = cpu.r[rm] };
-                            }
-                        },
+                        const rm_idx = opcode & 0xF;
 
-                        else => unreachable,
+                        if (R) {
+                            std.debug.panic("[CPU/PSR Transfer] TODO: MSR on SPSR_<current_mode> is unimplemented", .{});
+                        } else {
+                            cpu.cpsr.raw = fieldMask(&cpu.cpsr, field_mask, cpu.r[rm_idx]);
+                        }
                     }
                 },
-                else => unreachable,
+                else => std.debug.panic("[CPU/PSR Transfer] Bits 21:220 of {X:0>8} are undefined", .{opcode}),
             }
         }
     }.inner;
 }
+
+fn fieldMask(psr: *const PSR, field_mask: u4, right: u32) u32 {
+    const bits = @truncate(u2, (field_mask >> 2 & 0x2) | (field_mask & 1));
+
+    const mask: u32 = switch (bits) {
+        0b00 => 0x0000_0000,
+        0b01 => 0x0000_00FF,
+        0b10 => 0xF000_0000,
+        0b11 => 0xF000_00FF,
+    };
+
+    return (psr.raw & ~mask) | (right & mask);
+}