From fc5a3460dd32e34794f175a668467299d2d1098b Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Tue, 18 Jan 2022 20:17:00 -0400
Subject: [PATCH] fix(cpu): improve MRS and MSR instructions

---
 src/cpu.zig                  |  5 +--
 src/cpu/arm/psr_transfer.zig | 66 +++++++++++++++++++++---------------
 2 files changed, 41 insertions(+), 30 deletions(-)

diff --git a/src/cpu.zig b/src/cpu.zig
index 168a240..f2ccbd1 100644
--- a/src/cpu.zig
+++ b/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) {
diff --git a/src/cpu/arm/psr_transfer.zig b/src/cpu/arm/psr_transfer.zig
index 6700ad8..e669d54 100644
--- a/src/cpu/arm/psr_transfer.zig
+++ b/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", .{});
-                            } else {
-                                const mask: u32 = 0xF000_0000;
-                                cpu.cpsr.raw = (cpu.cpsr.raw & ~mask) | (right & mask);
-                            }
-                        },
-                        0b001 => {
-                            if (isSpsr) {
-                                std.debug.panic("[CPU] TODO: MSR on SPSR_<current_mode> is unimplemented", .{});
-                            } else {
-                                cpu.cpsr = .{ .raw = cpu.r[rm] };
-                            }
-                        },
+                        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;
 
-                        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);
+}