fix(cpu): implement S set + rd == 15 case for data processing

src/cpu.zig

@@ -33,6 +33,17 @@ pub const Arm7tdmi = struct {
     sched: *Scheduler,
     bus: *Bus,
     cpsr: PSR,
+    spsr: PSR,
+
+    /// Storage  for R8_fiq -> R12_fiq and their normal counterparts
+    /// e.g [r[0 + 8], fiq_r[0 + 8], r[1 + 8], fiq_r[1 + 8]...]
+    banked_fiq: [2 * 5]u32,
+
+    /// Storage for r13_<mode>, r14_<mode>
+    /// e.g. [r13, r14, r13_svc, r14_svc]
+    banked_r: [2 * 6]u32,
+
+    banked_spsr: [5]PSR,
 
     pub fn init(sched: *Scheduler, bus: *Bus) Self {
         return .{
@@ -40,9 +51,100 @@ pub const Arm7tdmi = struct {
             .sched = sched,
             .bus = bus,
             .cpsr = .{ .raw = 0x0000_00DF },
+            .spsr = .{ .raw = 0x0000_0000 },
+            .banked_fiq = [_]u32{0x00} ** 10,
+            .banked_r = [_]u32{0x00} ** 12,
+            .banked_spsr = [_]PSR{.{ .raw = 0x0000_0000 }} ** 5,
         };
     }
 
+    fn bankedIdx(mode: Mode) usize {
+        return switch (mode) {
+            .User, .System => 0,
+            .Supervisor => 1,
+            .Abort => 2,
+            .Undefined => 3,
+            .IRQ => 4,
+            .FIQ => 5,
+        };
+    }
+
+    fn spsrIdx(mode: Mode) usize {
+        return switch (mode) {
+            .Supervisor => 0,
+            .Abort => 1,
+            .Undefined => 2,
+            .IRQ => 3,
+            .FIQ => 4,
+            else => std.debug.panic("{} does not have a SPSR Register", .{mode}),
+        };
+    }
+
+    pub fn hasSPSR(self: *const Self) bool {
+        return switch (getMode(self.cpsr.mode.read())) {
+            .System, .User => false,
+            else => true,
+        };
+    }
+
+    pub fn isPrivileged(self: *const Self) bool {
+        return switch (getMode(self.cpsr.mode.read())) {
+            .User => false,
+            else => true,
+        };
+    }
+
+    pub fn setCpsr(self: *Self, value: u32) void {
+        if (value & 0x1F != self.cpsr.raw & 0x1F) self.changeMode(@truncate(u5, value & 0x1F));
+        self.cpsr.raw = value;
+    }
+
+    fn changeMode(self: *Self, next_idx: u5) void {
+        const next = getMode(next_idx);
+        const now = getMode(self.cpsr.mode.read());
+
+        // Bank R8 -> r12
+        var r: usize = 8;
+        while (r <= 12) : (r += 1) {
+            self.banked_fiq[(r - 8) * 2 + if (now == .FIQ) @as(usize, 1) else 0] = self.r[r];
+        }
+
+        // Bank r13, r14, SPSR
+        switch (now) {
+            .User, .System => {
+                self.banked_r[bankedIdx(now) * 2 + 0] = self.r[13];
+                self.banked_r[bankedIdx(now) * 2 + 1] = self.r[14];
+            },
+            else => {
+                self.banked_r[bankedIdx(now) * 2 + 0] = self.r[13];
+                self.banked_r[bankedIdx(now) * 2 + 1] = self.r[14];
+                self.banked_spsr[spsrIdx(now)] = self.spsr;
+            },
+        }
+
+        // Grab R8 -> R12
+        r = 8;
+        while (r <= 12) : (r += 1) {
+            self.r[r] = self.banked_fiq[(r - 8) * 2 + if (next == .FIQ) @as(usize, 1) else 0];
+        }
+
+        // Grab r13, r14, SPSR
+        switch (next) {
+            .User, .System => {
+                self.r[13] = self.banked_r[bankedIdx(next) * 2 + 0];
+                self.r[14] = self.banked_r[bankedIdx(next) * 2 + 1];
+                // FIXME: Should we clear out SPSR?
+            },
+            else => {
+                self.r[13] = self.banked_r[bankedIdx(next) * 2 + 0];
+                self.r[14] = self.banked_r[bankedIdx(next) * 2 + 1];
+                self.spsr = self.banked_spsr[spsrIdx(next)];
+            },
+        }
+
+        self.cpsr.mode.write(next_idx);
+    }
+
     pub fn skipBios(self: *Self) void {
         self.r[0] = 0x08000000;
         self.r[1] = 0x000000EA;
@@ -112,9 +214,9 @@ pub const Arm7tdmi = struct {
         const r14 = self.r[14];
         const r15 = self.r[15];
 
-        const cpsr = self.cpsr.raw;
+        const c_psr = self.cpsr.raw;
 
-        nosuspend stderr.print("{X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} cpsr: {X:0>8} | ", .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, cpsr }) catch return;
+        nosuspend stderr.print("{X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} {X:0>8} cpsr: {X:0>8} | ", .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, c_psr }) catch return;
         nosuspend if (self.cpsr.t.read()) stderr.print("{X:0>4}:\n", .{@truncate(u16, opcode)}) catch return else stderr.print("{X:0>8}:\n", .{opcode}) catch return;
     }
 };
@@ -274,6 +376,10 @@ const Mode = enum(u5) {
     System = 0b11111,
 };
 
+pub fn getMode(bits: u5) Mode {
+    return std.meta.intToEnum(Mode, bits) catch unreachable;
+}
+
 fn armUndefined(_: *Arm7tdmi, _: *Bus, opcode: u32) void {
     const id = armIdx(opcode);
     std.debug.panic("[CPU:ARM] ID: 0x{X:0>3} 0x{X:0>8} is an illegal opcode", .{ id, opcode });

src/cpu/arm/data_processing.zig

@@ -27,38 +27,32 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                     // 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);
-                        // C set by Barrel Shifter, V is unaffected
-                    }
+                    logicFlags(S, cpu, rd, result);
                 },
                 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
-                    }
+                    logicFlags(S, cpu, rd, result);
                 },
-                0x2 => sub(S, cpu, rd, op1, op2), // SUB
-                0x3 => sub(S, cpu, rd, op2, op1), // RSB
+                0x2 => cpu.r[rd] = sub(S, cpu, rd, op1, op2), // SUB
+                0x3 => cpu.r[rd] = sub(S, cpu, rd, op2, op1), // RSB
                 0x4 => {
                     // ADD
                     var result: u32 = undefined;
                     const didOverflow = @addWithOverflow(u32, op1, op2, &result);
                     cpu.r[rd] = result;
 
-                    if (S and rd != 0xF) {
+                    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(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
                         }
+                    }
                 },
                 0x5 => {
                     // ADC
@@ -68,32 +62,28 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                     const overflow = @addWithOverflow(u32, result, old_carry, &result);
                     cpu.r[rd] = result;
 
-                    if (S and rd != 0xF) {
+                    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);
                         }
+                    }
                 },
-                0x6 => sbc(S, cpu, rd, op1, op2, old_carry), // SBC
-                0x7 => sbc(S, cpu, rd, op2, op1, old_carry), // RSC
+                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
                     const result = op1 & op2;
-
-                    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) _ = shifter.execute(true, cpu, opcode);
+                    testFlags(S, cpu, opcode, result);
                 },
                 0x9 => {
                     // TEQ
                     const result = op1 ^ op2;
-
-                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
-                    cpu.cpsr.z.write(result == 0);
-                    // Barrel Shifter should always calc CPSR C in TEQ
-                    if (!S) _ = shifter.execute(true, cpu, opcode);
+                    testFlags(S, cpu, opcode, result);
                 },
                 0xA => {
                     // CMP
@@ -118,72 +108,81 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                     // 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
-                    }
+                    logicFlags(S, cpu, rd, result);
                 },
                 0xD => {
                     // MOV
                     cpu.r[rd] = op2;
-
-                    if (S and rd != 0xF) {
-                        cpu.cpsr.n.write(op2 >> 31 & 1 == 1);
-                        cpu.cpsr.z.write(op2 == 0);
-                        // C set by Barrel Shifter, V is unaffected
-                    }
+                    logicFlags(S, cpu, rd, op2);
                 },
                 0xE => {
                     // BIC
                     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
-                    }
+                    logicFlags(S, cpu, rd, result);
                 },
                 0xF => {
                     // MVN
                     const result = ~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
-                    }
+                    logicFlags(S, cpu, rd, result);
                 },
             }
         }
     }.inner;
 }
 
-fn sbc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32, old_carry: u1) void {
+fn sbc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, 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);
-    cpu.r[rd] = result;
 
-    if (S and rd != 0xF) {
+    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);
         }
+    }
+
+    return result;
 }
 
-fn sub(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) void {
+fn sub(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
     const result = left -% right;
-    cpu.r[rd] = result;
 
-    if (S and rd != 0xF) {
+    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);
         }
+    }
+
+    return result;
+}
+
+fn logicFlags(comptime S: bool, cpu: *Arm7tdmi, rd: u4, result: u32) void {
+    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
+        }
+    }
+}
+
+fn testFlags(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) _ = shifter.execute(true, cpu, opcode);
 }

src/cpu/arm/psr_transfer.zig

@@ -13,32 +13,21 @@ pub fn psrTransfer(comptime I: bool, comptime R: bool, comptime kind: u2) InstrF
                     // MRS
                     const rd = opcode >> 12 & 0xF;
 
-                    if (R) {
-                        std.debug.panic("[CPU/PSR Transfer] TODO: MRS on SPSR_<current_mode> is unimplemented", .{});
-                    } else {
-                        cpu.r[rd] = cpu.cpsr.raw;
-                    }
+                    if (R and !cpu.hasSPSR()) std.log.warn("[CPU/PSR Transfer] Tried to read SPSR from User/System Mode", .{});
+                    cpu.r[rd] = if (R) cpu.spsr.raw else cpu.cpsr.raw;
                 },
                 0b10 => {
                     // MSR
                     const field_mask = @truncate(u4, opcode >> 16 & 0xF);
-
-                    if (I) {
-                        const imm = std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1);
-
-                        if (R) {
-                            std.debug.panic("[CPU/PSR Transfer] TODO: MSR (flags only) on SPSR_<current_mode> is unimplemented", .{});
-                        } else {
-                            cpu.cpsr.raw = fieldMask(&cpu.cpsr, field_mask, imm);
-                        }
-                    } else {
                     const rm_idx = opcode & 0xF;
+                    const right = if (I) std.math.rotr(u32, opcode & 0xFF, (opcode >> 8 & 0xF) << 1) else cpu.r[rm_idx];
+
+                    if (R and !cpu.hasSPSR()) std.log.warn("[CPU/PSR Transfer] Tried to write to SPSR User/System Mode", .{});
 
                     if (R) {
-                            std.debug.panic("[CPU/PSR Transfer] TODO: MSR on SPSR_<current_mode> is unimplemented", .{});
+                        if (cpu.isPrivileged()) cpu.spsr.raw = fieldMask(&cpu.spsr, field_mask, right);
                     } else {
-                            cpu.cpsr.raw = fieldMask(&cpu.cpsr, field_mask, cpu.r[rm_idx]);
-                        }
+                        if (cpu.isPrivileged()) cpu.setCpsr(fieldMask(&cpu.cpsr, field_mask, right));
                     }
                 },
                 else => std.debug.panic("[CPU/PSR Transfer] Bits 21:220 of {X:0>8} are undefined", .{opcode}),