Fix VDP DMA Memory to VRAM source address

2 files changed, 88 insertions, 69 deletions

diff --git a/vdp.cpp b/vdp.cpp
index 6acb7ea..de4d05d 100644
--- a/vdp.cpp
+++ b/vdp.cpp
@@ -7,7 +7,6 @@
 
 #include <cassert>
 #include <cstring>
-#include <cstdio>
 
 #define REG_VAL_GET(reg, mask, shift) (((reg) & ((mask) << (shift))) >> (shift))
 #define REG_VAL_SET(reg, mask, shift, v) \
@@ -210,69 +209,6 @@ static void TraceRegBits(FILE* s, const char* bits_desc[16], const uint16_t val)
     fprintf(s, ")");
 }
 
-static void TraceRegWrite(FILE* const s, const uint8_t reg, const uint8_t val)
-{
-    const auto reg_id = static_cast<VDP::RegID>(reg);
-    fprintf(s, ": reg 0x%02x(%s) = 0x%02x", reg, VDP::RegIDToString(reg_id), val);
-    switch (reg_id) {
-    case VDP::RegID::kModeSet1:
-        return TraceRegBits(s, reg_mode_set_1_bits_desc, val);
-    case VDP::RegID::kModeSet2:
-        return TraceRegBits(s, reg_mode_set_2_bits_desc, val);
-    case VDP::RegID::kScrollAAddress:
-        fprintf(s, " (0x%04x)", uint16_t(val & 0x38) << 10);
-        break;
-    case VDP::RegID::kWindowAddress:
-        fprintf(s, " (0x%04x)", uint16_t(val & 0x3e) << 10);
-        break;
-    case VDP::RegID::kScrollBAddress:
-        fprintf(s, " (0x%04x)", uint16_t(val & 0x7) << 13);
-        break;
-    case VDP::RegID::kSpritesTableAddress:
-        fprintf(s, " (0x%04x)", uint16_t(val & 0x7f) << 9);
-        break;
-    case VDP::RegID::kBackgroundColor:
-        break;
-    case VDP::RegID::kHint:
-        break;
-    case VDP::RegID::kModeSet3:
-        return TraceRegBits(s, reg_mode_set_3_bits_desc, val);
-    case VDP::RegID::kModeSet4:
-        return TraceRegBits(s, reg_mode_set_4_bits_desc, val);
-    case VDP::RegID::kHScrollTableAddress:
-        fprintf(s, " (0x%04x)", uint16_t(val & 0x3f) << 10);
-        break;
-    case VDP::RegID::kAutoIncrement:
-        break;
-    case VDP::RegID::kScrollSize:
-        break;
-    case VDP::RegID::kWindowHorizontalPosition:
-        break;
-    case VDP::RegID::kWindowVertialPosition:
-        break;
-    case VDP::RegID::kDMALengthCounterLow:
-        break;
-    case VDP::RegID::kDMALengthCounterHigh:
-        break;
-    case VDP::RegID::kDMASourceAddressLow:
-        fprintf(s, " (0xXXXX%02x)", val);
-        break;
-    case VDP::RegID::kDMASourceAddressMid:
-        fprintf(s, " (0xXX%02xXX)", val);
-        break;
-    case VDP::RegID::kDMASourceAddressHigh:
-        {
-            const char* op = (val & 0x80)
-                ? ((val & 0x40) ? "VRAM Copy" : "VRAM Fill")
-                : "MemToVRAM";
-            fprintf(s, " (%s 0x%02xXXXX)", op, val & ((val & 0x80) ? 0x3f : 0x7f));
-        }
-        break;
-    case VDP::RegID::kRegistersCount:
-        break;
-    }
-}
-
 uint32_t VDP::Read(const uint32_t offset, const enum bitness bitness)
 {
     uint32_t ret{};
@@ -737,10 +673,10 @@ void VDP::writeControl(const uint16_t value)
                     if (DEBUG_TRACE_VDP_ACCESS) {
                         printf(": Running DMA Memory to VRAM");
                     }
-                    const uint32_t source_address =
-                        _reg[static_cast<size_t>(RegID::kDMASourceAddressLow)] |
-                        (_reg[static_cast<size_t>(RegID::kDMASourceAddressMid)] << 8) |
-                        ((_reg[static_cast<size_t>(RegID::kDMASourceAddressHigh)] & 0x3f) << 16);
+                    const uint32_t hi = _reg[static_cast<size_t>(RegID::kDMASourceAddressHigh)];
+                    const uint32_t mid = _reg[static_cast<size_t>(RegID::kDMASourceAddressMid)];
+                    const uint32_t lo = _reg[static_cast<size_t>(RegID::kDMASourceAddressLow)];
+                    const uint32_t source_address = (((hi & 0x7f) << 16) | (mid << 8) | lo) << 1;
                     runDMAMemoryToVRAM(
                             baseFromAddressMode(_address_mode),
                             source_address,
@@ -776,7 +712,7 @@ void VDP::writeControl(const uint16_t value)
         const uint8_t reg_num = (value >> 8) & 0x1f;
         const uint8_t reg_val = value & 0xff;
         if (DEBUG_TRACE_VDP_ACCESS) {
-            TraceRegWrite(stdout, reg_num, reg_val);
+            traceRegWrite(stdout, reg_num, reg_val);
         }
         _reg[reg_num] = reg_val;
         // TODO print warnings of all invalid values or writes that ignore a mask
@@ -1026,6 +962,87 @@ const char* VDP::RegIDToString(const RegID reg_id)
     return "Unknown";
 }
 
+void VDP::traceRegWrite(FILE* const s, const uint8_t reg, const uint8_t val) const
+{
+    const auto reg_id = static_cast<VDP::RegID>(reg);
+    fprintf(s, ": reg 0x%02x(%s) = 0x%02x", reg, VDP::RegIDToString(reg_id), val);
+    switch (reg_id) {
+    case VDP::RegID::kModeSet1:
+        return TraceRegBits(s, reg_mode_set_1_bits_desc, val);
+    case VDP::RegID::kModeSet2:
+        return TraceRegBits(s, reg_mode_set_2_bits_desc, val);
+    case VDP::RegID::kScrollAAddress:
+        fprintf(s, " (0x%04x)", uint16_t(val & 0x38) << 10);
+        break;
+    case VDP::RegID::kWindowAddress:
+        fprintf(s, " (0x%04x)", uint16_t(val & 0x3e) << 10);
+        break;
+    case VDP::RegID::kScrollBAddress:
+        fprintf(s, " (0x%04x)", uint16_t(val & 0x7) << 13);
+        break;
+    case VDP::RegID::kSpritesTableAddress:
+        fprintf(s, " (0x%04x)", uint16_t(val & 0x7f) << 9);
+        break;
+    case VDP::RegID::kBackgroundColor:
+        break;
+    case VDP::RegID::kHint:
+        break;
+    case VDP::RegID::kModeSet3:
+        return TraceRegBits(s, reg_mode_set_3_bits_desc, val);
+    case VDP::RegID::kModeSet4:
+        return TraceRegBits(s, reg_mode_set_4_bits_desc, val);
+    case VDP::RegID::kHScrollTableAddress:
+        fprintf(s, " (0x%04x)", uint16_t(val & 0x3f) << 10);
+        break;
+    case VDP::RegID::kAutoIncrement:
+        break;
+    case VDP::RegID::kScrollSize:
+        break;
+    case VDP::RegID::kWindowHorizontalPosition:
+        break;
+    case VDP::RegID::kWindowVertialPosition:
+        break;
+    case VDP::RegID::kDMALengthCounterLow:
+        break;
+    case VDP::RegID::kDMALengthCounterHigh:
+        break;
+    case VDP::RegID::kDMASourceAddressLow:
+        {
+            const uint32_t hi_raw = _reg[static_cast<size_t>(RegID::kDMASourceAddressHigh)];
+            const uint32_t hi = (hi_raw & ((hi_raw & 0x80) ? 0x3f : 0x7f));
+            const uint32_t mid = _reg[static_cast<size_t>(RegID::kDMASourceAddressMid)];
+            const uint32_t lo = val;
+            const uint32_t addr = ((hi << 16) | (mid << 8) | lo) << 1;
+            fprintf(s, " (0x%08x)", addr);
+        }
+        break;
+    case VDP::RegID::kDMASourceAddressMid:
+        {
+            const uint32_t hi_raw = _reg[static_cast<size_t>(RegID::kDMASourceAddressHigh)];
+            const uint32_t hi = (hi_raw & ((hi_raw & 0x80) ? 0x3f : 0x7f));
+            const uint32_t mid = val;
+            const uint32_t lo = _reg[static_cast<size_t>(RegID::kDMASourceAddressLow)];
+            const uint32_t addr = ((hi << 16) | (mid << 8) | lo) << 1;
+            fprintf(s, " (0x%08x)", addr);
+        }
+        break;
+    case VDP::RegID::kDMASourceAddressHigh:
+        {
+            const char* op = (val & 0x80)
+                ? ((val & 0x40) ? "VRAM Copy" : "VRAM Fill")
+                : "MemToVRAM";
+            const uint32_t hi = (val & ((val & 0x80) ? 0x3f : 0x7f));
+            const uint32_t mid = _reg[static_cast<size_t>(RegID::kDMASourceAddressMid)];
+            const uint32_t lo = _reg[static_cast<size_t>(RegID::kDMASourceAddressLow)];
+            const uint32_t addr = ((hi << 16) | (mid << 8) | lo) << 1;
+            fprintf(s, " (%s 0x%08x)", op, addr);
+        }
+        break;
+    case VDP::RegID::kRegistersCount:
+        break;
+    }
+}
+
 void VDP::runDMAMemoryToVRAM(
         uint8_t* const base,
         const uint32_t source_address,
diff --git a/vdp.hpp b/vdp.hpp
index 1b86784..1b67625 100644
--- a/vdp.hpp
+++ b/vdp.hpp
@@ -7,6 +7,7 @@
 
 #include <cstddef>
 #include <cstring>
+#include <cstdio>
 
 struct SpriteAttributeEntry {
     uint16_t vpos;
@@ -102,6 +103,7 @@ private:
     uint16_t readStatusRegister() const;
     uint8_t* baseFromAddressMode(uint8_t address_mode);
     const char* addressModeToString(uint8_t address_mode);
+    void traceRegWrite(FILE* const s, const uint8_t reg, const uint8_t val) const;
 
     static void runDMAMemoryToVRAM(
             uint8_t* base,