From 0029e302b2d23546c7687f88dd22c7db9f925665 Mon Sep 17 00:00:00 2001
From: Amneesh Singh <natto@weirdnatto.in>
Date: Thu, 13 Jun 2024 03:54:12 +0530
Subject: [PATCH] cpu/arm: fix block data transfer

Signed-off-by: Amneesh Singh <natto@weirdnatto.in>
---
 src/cpu/arm/exec.cc   | 54 +++++++++++++++++++++++++------------------
 tests/cpu/arm/exec.cc | 10 ++++----
 2 files changed, 36 insertions(+), 28 deletions(-)

diff --git a/src/cpu/arm/exec.cc b/src/cpu/arm/exec.cc
index 072ff08..df09431 100644
--- a/src/cpu/arm/exec.cc
+++ b/src/cpu/arm/exec.cc
@@ -283,8 +283,7 @@ Instruction::exec(Cpu& cpu) {
 
             uint32_t address = cpu.gpr[data.rn];
             Mode mode        = cpu.cpsr.mode();
-            uint8_t i        = 0;
-            uint8_t n_regs   = std::popcount(data.regs);
+            int8_t i        = 0;
 
             pc_error(data.rn);
 
@@ -304,11 +303,7 @@ Instruction::exec(Cpu& cpu) {
                 }
             }
 
-            // TODO: clean this shit
-            // account for decrement
-            if (!data.up)
-                address -= (n_regs - 1) * alignment;
-
+            // increment beforehand
             if (data.pre)
                 address += (data.up ? alignment : -alignment);
 
@@ -319,29 +314,42 @@ Instruction::exec(Cpu& cpu) {
                     cpu.spsr = cpu.cpsr;
                 }
 
-                for (i = 0; i < cpu.GPR_COUNT; i++) {
-                    if (get_bit(data.regs, i)) {
-                        cpu.gpr[i] = cpu.bus->read_word(address);
-                        address += alignment;
+                if (data.up) {
+                    for (i = 0; i < cpu.GPR_COUNT; i++) {
+                        if (get_bit(data.regs, i)) {
+                            cpu.gpr[i] = cpu.bus->read_word(address);
+                            address += alignment;
+                        }
+                    }
+                } else {
+                    for (i = cpu.GPR_COUNT - 1; i >= 0; i--) {
+                        if (get_bit(data.regs, i)) {
+                            cpu.gpr[i] = cpu.bus->read_word(address);
+                            address -= alignment;
+                        }
                     }
                 }
             } else {
-                for (i = 0; i < cpu.GPR_COUNT; i++) {
-                    if (get_bit(data.regs, i)) {
-                        cpu.bus->write_word(address, cpu.gpr[i]);
-                        address += alignment;
+                if (data.up) {
+                    for (i = 0; i < cpu.GPR_COUNT; i++) {
+                        if (get_bit(data.regs, i)) {
+                            cpu.bus->write_word(address, cpu.gpr[i]);
+                            address += alignment;
+                        }
+                    }
+                } else {
+                    for (i = cpu.GPR_COUNT - 1; i >= 0; i--) {
+                        if (get_bit(data.regs, i)) {
+                            cpu.bus->write_word(address, cpu.gpr[i]);
+                            address -= alignment;
+                        }
                     }
                 }
             }
 
-            if (!data.pre)
-                address += (data.up ? alignment : -alignment);
-
-            // reset back to original address + offset if incremented earlier
-            if (data.up)
-                address -= n_regs * alignment;
-            else
-                address -= alignment;
+            // fix increment
+            if (data.pre)
+                address += (data.up ? -alignment : alignment);
 
             if (!data.pre || data.write)
                 cpu.gpr[data.rn] = address;
diff --git a/tests/cpu/arm/exec.cc b/tests/cpu/arm/exec.cc
index 14649f3..b750153 100644
--- a/tests/cpu/arm/exec.cc
+++ b/tests/cpu/arm/exec.cc
@@ -557,7 +557,7 @@ TEST_CASE_METHOD(CpuFixture, "Block Data Transfer", TAG) {
         setr(10, address);
         block_transfer->write = true;
         exec(data);
-        checker(address + alignment);
+        checker(address + 7 * alignment);
 
         // decrement
         block_transfer->write = false;
@@ -568,10 +568,10 @@ TEST_CASE_METHOD(CpuFixture, "Block Data Transfer", TAG) {
         checker(address + alignment * 8);
 
         // with write
-        setr(10, 0x3000D98);
+        setr(10, address + alignment * 8);
         block_transfer->write = true;
         exec(data);
-        checker(address + alignment * 7);
+        checker(address + alignment);
 
         // post increment
         block_transfer->write = false;
@@ -580,14 +580,14 @@ TEST_CASE_METHOD(CpuFixture, "Block Data Transfer", TAG) {
         // adjust rn
         setr(10, address + alignment);
         exec(data);
-        checker(address + alignment * 2);
+        checker(address + alignment * 8);
 
         // post decrement
         block_transfer->up = false;
         // adjust rn
         setr(10, address + alignment * 7);
         exec(data);
-        checker(address + alignment * 6);
+        checker(address);
 
         // with s bit
         cpu.chg_mode(Mode::Fiq);