From b9255bffe7638ebb1374cc88aa1276543f87d5b5 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Fri, 21 Oct 2022 05:11:55 -0300
Subject: [PATCH] feat(cpu): implement MSR and MRS

---
 src/cpu.zig                      |  6 ++---
 src/cpu/barrel_shifter.zig       |  2 +-
 src/cpu/psr_transfer.zig         | 45 +++++++++++++++++++++++++++++---
 src/cpu/single_data_transfer.zig |  2 +-
 4 files changed, 47 insertions(+), 8 deletions(-)

diff --git a/src/cpu.zig b/src/cpu.zig
index 09bb9b1..7715ce7 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -23,7 +23,7 @@ pub const Arm7tdmi = struct {
     r: [16]u32,
     sched: *Scheduler,
     bus: *Bus,
-    cpsr: CPSR,
+    cpsr: PSR,
 
     pub fn init(sched: *Scheduler, bus: *Bus) Self {
         return .{
@@ -97,7 +97,7 @@ fn armIdx(opcode: u32) u12 {
     return @truncate(u12, opcode >> 20 & 0xFF) << 4 | @truncate(u12, opcode >> 4 & 0xF);
 }
 
-fn checkCond(cpsr: *const CPSR, opcode: u32) bool {
+fn checkCond(cpsr: *const PSR, opcode: u32) bool {
     // TODO: Should I implement an enum?
     return switch (@truncate(u4, opcode >> 28)) {
         0x0 => cpsr.z.read(), // EQ - Equal
@@ -183,7 +183,7 @@ fn populate() [0x1000]InstrFn {
     };
 }
 
-pub const CPSR = extern union {
+pub const PSR = extern union {
     mode: Bitfield(u32, 0, 5),
     t: Bit(u32, 5),
     f: Bit(u32, 6),
diff --git a/src/cpu/barrel_shifter.zig b/src/cpu/barrel_shifter.zig
index d2bb272..4ab804f 100644
--- a/src/cpu/barrel_shifter.zig
+++ b/src/cpu/barrel_shifter.zig
@@ -1,7 +1,7 @@
 const std = @import("std");
 
 const Arm7tdmi = @import("../cpu.zig").Arm7tdmi;
-const CPSR = @import("../cpu.zig").CPSR;
+const CPSR = @import("../cpu.zig").PSR;
 
 pub fn exec(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
     var shift_amt: u8 = undefined;
diff --git a/src/cpu/psr_transfer.zig b/src/cpu/psr_transfer.zig
index 1e5aab2..e080c8d 100644
--- a/src/cpu/psr_transfer.zig
+++ b/src/cpu/psr_transfer.zig
@@ -4,10 +4,49 @@ const Bus = @import("../Bus.zig");
 const Arm7tdmi = @import("../cpu.zig").Arm7tdmi;
 const InstrFn = @import("../cpu.zig").InstrFn;
 
-pub fn psrTransfer(comptime _: bool, comptime _: bool) InstrFn {
+pub fn psrTransfer(comptime I: bool, comptime isSpsr: bool) InstrFn {
     return struct {
-        fn inner(_: *Arm7tdmi, _: *Bus, _: u32) void {
-            std.debug.panic("[CPU] TODO: Implement PSR Transfer Instructions", .{});
+        fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
+            switch (@truncate(u3, opcode >> 19)) {
+                0b001 => {
+                    // MRS
+                    const rn = opcode >> 12 & 0xF;
+
+                    if (isSpsr) {
+                        std.debug.panic("[CPU] TODO: MRS on SPSR_<current_mode> is unimplemented", .{});
+                    } else {
+                        cpu.r[rn] = cpu.cpsr.raw;
+                    }
+                },
+                0b101 => {
+                    // MSR
+                    const rm = opcode & 0xF;
+
+                    switch (@truncate(u3, opcode >> 16)) {
+                        0b000 => {
+                            if (isSpsr) {
+                                std.debug.panic("[CPU] TODO: MSR on SPSR_<current_mode> is unimplemented", .{});
+                            } else {
+                                cpu.cpsr = .{ .raw = cpu.r[rm] };
+                            }
+                        },
+                        0b001 => {
+                            const right = if (I) std.math.rotr(u32, opcode & 0xFF, opcode >> 8 & 0xF) else cpu.r[rm];
+
+                            if (isSpsr) {
+                                std.debug.panic("[CPU] TODO: MSR (flags only) on SPSR_<current_mode> is unimplemented", .{});
+                            } else {
+                                cpu.cpsr.n.write(right >> 31 & 1 == 1);
+                                cpu.cpsr.z.write(right >> 30 & 1 == 1);
+                                cpu.cpsr.c.write(right >> 29 & 1 == 1);
+                                cpu.cpsr.v.write(right >> 28 & 1 == 1);
+                            }
+                        },
+                        else => unreachable,
+                    }
+                },
+                else => unreachable,
+            }
         }
     }.inner;
 }
diff --git a/src/cpu/single_data_transfer.zig b/src/cpu/single_data_transfer.zig
index 17daf5f..c3e83f6 100644
--- a/src/cpu/single_data_transfer.zig
+++ b/src/cpu/single_data_transfer.zig
@@ -4,7 +4,7 @@ const util = @import("../util.zig");
 const BarrelShifter = @import("barrel_shifter.zig");
 const Bus = @import("../Bus.zig");
 const Arm7tdmi = @import("../cpu.zig").Arm7tdmi;
-const CPSR = @import("../cpu.zig").CPSR;
+const CPSR = @import("../cpu.zig").PSR;
 const InstrFn = @import("../cpu.zig").InstrFn;
 
 pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool, comptime B: bool, comptime W: bool, comptime L: bool) InstrFn {