From 85e0924669b307a155fd216f966aa797eca8544e Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Fri, 21 Oct 2022 05:12:10 -0300
Subject: [PATCH] feat: implement LDM/STM behaviour when S is set

---
 src/cpu.zig                         | 43 +++++++++++++++++++++++++++++
 src/cpu/arm/block_data_transfer.zig | 35 ++++++++++++-----------
 2 files changed, 62 insertions(+), 16 deletions(-)

diff --git a/src/cpu.zig b/src/cpu.zig
index 0de4bb5..c02ce1b 100644
--- a/src/cpu.zig
+++ b/src/cpu.zig
@@ -137,6 +137,49 @@ pub const Arm7tdmi = struct {
         self.changeMode(mode);
     }
 
+    pub fn setUserModeRegister(self: *Self, idx: usize, value: u32) void {
+        const current = getMode(self.cpsr.mode.read()) orelse unreachable;
+
+        if (idx < 8) {
+            self.r[idx] = value;
+        } else if (idx < 13) {
+            if (current == .Fiq) {
+                const user_offset: usize = 0;
+                self.banked_fiq[(idx - 8) * 2 + user_offset] = value;
+            } else self.r[idx] = value;
+        } else if (idx < 15) {
+            switch (current) {
+                .User, .System => self.r[idx] = value,
+                else => {
+                    self.banked_r[bankedIdx(.User) * 2 + (idx - 13)] = value;
+                },
+            }
+        } else self.r[idx] = value;
+    }
+
+    pub fn getUserModeRegister(self: *Self, idx: usize) u32 {
+        const current = getMode(self.cpsr.mode.read()) orelse unreachable;
+
+        var result: u32 = undefined;
+        if (idx < 8) {
+            result = self.r[idx];
+        } else if (idx < 13) {
+            if (current == .Fiq) {
+                const user_offset: usize = 0;
+                result = self.banked_fiq[(idx - 8) * 2 + user_offset];
+            } else result = self.r[idx];
+        } else if (idx < 15) {
+            switch (current) {
+                .User, .System => result = self.r[idx],
+                else => {
+                    result = self.banked_r[bankedIdx(.User) * 2 + (idx - 13)];
+                },
+            }
+        } else result = self.r[idx];
+
+        return result;
+    }
+
     pub fn changeMode(self: *Self, next: Mode) void {
         const now = getMode(self.cpsr.mode.read()) orelse unreachable;
 
diff --git a/src/cpu/arm/block_data_transfer.zig b/src/cpu/arm/block_data_transfer.zig
index 82681e0..bc4209c 100644
--- a/src/cpu/arm/block_data_transfer.zig
+++ b/src/cpu/arm/block_data_transfer.zig
@@ -7,11 +7,10 @@ const InstrFn = @import("../../cpu.zig").ArmInstrFn;
 pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, comptime W: bool, comptime L: bool) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, bus: *Bus, opcode: u32) void {
+            const r15_present = opcode >> 15 & 1 == 1;
             const rn = opcode >> 16 & 0xF;
             const base = cpu.r[rn];
 
-            if (S and opcode >> 15 & 1 == 0) cpu.panic("[CPU] TODO: STM/LDM with S set but R15 not in transfer list", .{});
-
             var address: u32 = undefined;
             if (U) {
                 // Increment
@@ -20,7 +19,7 @@ pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, c
                 var i: u5 = 0;
                 while (i < 0x10) : (i += 1) {
                     if (opcode >> i & 1 == 1) {
-                        transfer(cpu, bus, i, address);
+                        transfer(cpu, bus, r15_present, i, address);
                         address += 4;
                     }
                 }
@@ -33,7 +32,7 @@ pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, c
                     const j = i - 1;
 
                     if (opcode >> j & 1 == 1) {
-                        transfer(cpu, bus, j, address);
+                        transfer(cpu, bus, r15_present, j, address);
                         address -= 4;
                     }
                 }
@@ -42,20 +41,24 @@ pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, c
             if (W and P or !P) cpu.r[rn] = if (U) address else address + 4;
         }
 
-        fn transfer(cpu: *Arm7tdmi, bus: *Bus, i: u5, address: u32) void {
+        fn transfer(cpu: *Arm7tdmi, bus: *Bus, r15_present: bool, i: u5, address: u32) void {
             if (L) {
-                cpu.r[i] = bus.read32(address);
-                if (S and i == 0xF) cpu.panic("[CPU] TODO: SPSR_<mode> is transferred to CPSR", .{});
-            } else {
-                if (i == 0xF) {
-                    if (!S) {
-                        // TODO: Assure that this is Address of STM instruction + 12
-                        bus.write32(address, cpu.r[i] + (12 - 4));
-                    } else {
-                        cpu.panic("[CPU] TODO: STM with S set and R15 in transfer list", .{});
-                    }
+                if (S and !r15_present) {
+                    // Always Transfer User mode Registers
+                    cpu.setUserModeRegister(i, bus.read32(address));
                 } else {
-                    bus.write32(address, cpu.r[i]);
+                    const value = bus.read32(address);
+                    cpu.r[i] = if (i == 0xF) value & 0xFFFF_FFFC else value;
+                    if (S and i == 0xF) cpu.setCpsr(cpu.spsr.raw);
+                }
+            } else {
+                if (S) {
+                    // Always Transfer User mode Registers
+                    // This happens regardless if r15 is in the list
+                    const value = cpu.getUserModeRegister(i);
+                    bus.write32(address, value + if (i == 0xF) 8 else @as(u32, 0)); // PC is already 4 ahead to make 12
+                } else {
+                    bus.write32(address, cpu.r[i] + if (i == 0xF) 8 else @as(u32, 0));
                 }
             }
         }