From 1ee8b51b2baa053bdafa695a88b55088a988a813 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Fri, 21 Oct 2022 05:12:17 -0300
Subject: [PATCH] chore: reimplement ARM LDM/STM

---
 src/cpu/arm/block_data_transfer.zig | 88 ++++++++++++++++++-----------
 1 file changed, 56 insertions(+), 32 deletions(-)

diff --git a/src/cpu/arm/block_data_transfer.zig b/src/cpu/arm/block_data_transfer.zig
index 16aaded..5bf4cb1 100644
--- a/src/cpu/arm/block_data_transfer.zig
+++ b/src/cpu/arm/block_data_transfer.zig
@@ -7,47 +7,71 @@ 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;
-            var address: u32 = cpu.r[rn];
+            const rn = @truncate(u4, opcode >> 16 & 0xF);
+            const rlist = opcode & 0xFFFF;
+            const r15 = rlist >> 15 & 1 == 1;
 
-            if (opcode & 0xFFFF == 0) {
-                if (L) cpu.r[15] = bus.read32(address) else bus.write32(address, cpu.r[15] + 8);
-                cpu.r[rn] += 0x40;
+            var count: u32 = 0;
+            var i: u5 = 0;
+            var first: u4 = 0;
+            var write_to_base = true;
+
+            while (i < 16) : (i += 1) {
+                const r = @truncate(u4, 15 - i);
+                if (rlist >> r & 1 == 1) {
+                    first = r;
+                    count += 1;
+                }
+            }
+
+            var start = cpu.r[rn];
+            if (U) {
+                start += if (P) 4 else 0;
+            } else {
+                start = start - (4 * count) + if (!P) 4 else 0;
+            }
+
+            var end = cpu.r[rn];
+            if (U) {
+                end = end + (4 * count) - if (!P) 4 else 0;
+            } else {
+                end -= if (P) 4 else 0;
+            }
+
+            var new_base = cpu.r[rn];
+            if (U) {
+                new_base += 4 * count;
+            } else {
+                new_base -= 4 * count;
+            }
+
+            var address = start;
+
+            if (rlist == 0) {
+                if (L) {
+                    cpu.r[15] = bus.read32(address);
+                } else {
+                    bus.write32(address, cpu.r[15] + 8);
+                }
+
+                cpu.r[rn] = if (U) cpu.r[rn] + 0x40 else cpu.r[rn] - 0x40;
                 return;
             }
 
-            if (U) {
-                // Increment
-                var i: u5 = 0;
-                while (i < 0x10) : (i += 1) {
-                    if (opcode >> i & 1 == 1) {
-                        if (P) address += 4;
-                        transfer(cpu, bus, r15_present, i, address);
-                        if (!P) address += 4;
-                    }
-                }
-            } else {
-                // Decrement
+            i = first;
+            while (i < 16) : (i += 1) {
+                if (rlist >> i & 1 == 1) {
+                    transfer(cpu, bus, r15, i, address);
+                    address += 4;
 
-                var i: u5 = 0x10;
-                while (i > 0) : (i -= 1) {
-                    const j = i - 1;
-
-                    if (opcode >> j & 1 == 1) {
-                        if (P) address -= 4;
-                        transfer(cpu, bus, r15_present, j, address);
-                        if (!P) address -= 4;
+                    if (W and !L and write_to_base) {
+                        cpu.r[rn] = new_base;
+                        write_to_base = false;
                     }
                 }
             }
 
-            if (W) {
-                const in_list = opcode >> @truncate(u4, rn) & 1 == 1;
-                if (!L or (L and !in_list)) {
-                    cpu.r[rn] = address;
-                }
-            }
+            if (W and L and opcode >> rn & 1 == 0) cpu.r[rn] = new_base;
         }
 
         fn transfer(cpu: *Arm7tdmi, bus: *Bus, r15_present: bool, i: u5, address: u32) void {