From 65e2f6469527165351666c3bf3675294cc6dc67a Mon Sep 17 00:00:00 2001
From: Jaklyy <102590697+Jaklyy@users.noreply.github.com>
Date: Sat, 21 Dec 2024 12:29:37 -0500
Subject: [PATCH] "immediate mode" dma start should be delayed 1 cycle

---
 src/ARM.h       |  11 ++
 src/CP15.cpp    | 299 ++++++++++++++++++++++++++++++++++++------------
 src/DMA.cpp     |  21 +++-
 src/DMA.h       |   2 +-
 src/NDS.cpp     |  16 ++-
 src/NDS.h       |  10 +-
 src/NDSCart.cpp |  10 +-
 7 files changed, 277 insertions(+), 92 deletions(-)

diff --git a/src/ARM.h b/src/ARM.h
index a7092e4c..49030d78 100644
--- a/src/ARM.h
+++ b/src/ARM.h
@@ -739,35 +739,46 @@ public:
     void JumpTo_4();
     void CodeRead32_2();
     void CodeRead32_3();
+    void CodeRead32_4();
     void ICacheLookup_2();
     void DAbortHandle();
     void DCacheFin8();
     void DRead8_2();
     void DRead8_3();
     void DRead8_4();
+    void DRead8_5();
     void DCacheFin16();
     void DRead16_2();
     void DRead16_3();
     void DRead16_4();
+    void DRead16_5();
     void DCacheFin32();
     void DRead32_2();
     void DRead32_3();
     void DRead32_4();
+    void DRead32_5();
     void DRead32S_2();
     void DRead32S_3();
     void DRead32S_4();
+    void DRead32S_5A();
+    void DRead32S_5B();
     void DWrite8_2();
     void DWrite8_3();
     void DWrite8_4();
+    void DWrite8_5();
     void DWrite16_2();
     void DWrite16_3();
     void DWrite16_4();
+    void DWrite16_5();
     void DWrite32_2();
     void DWrite32_3();
     void DWrite32_4();
+    void DWrite32_5();
     void DWrite32S_2();
     void DWrite32S_3();
     void DWrite32S_4();
+    void DWrite32S_5A();
+    void DWrite32S_5B();
     void WBCheck_2();
     void ICachePrefetch_2();
     void DCacheLookup_2();
diff --git a/src/CP15.cpp b/src/CP15.cpp
index e0ca7e83..bafa6a48 100644
--- a/src/CP15.cpp
+++ b/src/CP15.cpp
@@ -2242,32 +2242,42 @@ void ARMv5::CodeRead32_3()
 
     NDS.ARM9Timestamp = NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1) & ~((1<<NDS.ARM9ClockShift)-1);
 
-    u8 cycles = MemTimings[addr >> 14][1];
-
     if ((addr >> 24) == 0x02)
     {
         FetchAddr[16] = addr;
         MRTrack.Type = MainRAMType::Fetch;
         MRTrack.Var = MRCodeFetch | MR32;
+
+        QueueFunction(DelayedQueue);
     }
     else
     {
         if (((NDS.ARM9Timestamp <= WBReleaseTS) && (NDS.ARM9Regions[addr>>14] == WBLastRegion)) // check write buffer
          || (Store && (NDS.ARM9Regions[addr>>14] == DataRegion))) //check the actual store
             NDS.ARM9Timestamp += 1<<NDS.ARM9ClockShift;
-
-        NDS.ARM9Timestamp += cycles;
-
-        if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-            WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-
-        RetVal = BusRead32(addr);
+            
+        QueueFunction(&ARMv5::CodeRead32_4);
     }
+}
+
+void ARMv5::CodeRead32_4()
+{
+    u32 addr = FetchAddr[16];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    u8 cycles = MemTimings[addr >> 14][1];
+
+    NDS.ARM9Timestamp += cycles;
+
+    if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
 
     Store = false;
     DataRegion = Mem9_Null;
+
+    RetVal = BusRead32(addr);
     QueueFunction(DelayedQueue);
-    return;
 }
 
 
@@ -2393,18 +2403,31 @@ void ARMv5::DRead8_4()
     }
     else
     {
-        NDS.ARM9Timestamp += MemTimings[addr >> 14][0];
-        DataCycles = 3<<NDS.ARM9ClockShift;
         DataRegion = NDS.ARM9Regions[addr>>14];
         if ((NDS.ARM9Timestamp <= WBReleaseTS) && (DataRegion == WBLastRegion)) // check write buffer
             NDS.ARM9Timestamp += 1<<NDS.ARM9ClockShift;
-    
-        if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-            WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-        *val = BusRead8(addr);
+
+        QueueFunction(&ARMv5::DRead8_5);
     }
 }
 
+void ARMv5::DRead8_5()
+{
+    u8 reg = __builtin_ctz(LDRRegs);
+    u32 addr = FetchAddr[reg];
+    u32 dummy; u32* val = (LDRFailedRegs & (1<<reg)) ? &dummy : &R[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr >> 14][0];
+    DataCycles = 3<<NDS.ARM9ClockShift;
+    
+    if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    *val = BusRead8(addr);
+}
+
 void ARMv5::DCacheFin16()
 {
     u8 reg = __builtin_ctz(LDRRegs);
@@ -2517,18 +2540,31 @@ void ARMv5::DRead16_4()
     }
     else
     {
-        NDS.ARM9Timestamp += MemTimings[addr >> 14][0];
-        DataCycles = 3<<NDS.ARM9ClockShift;
         DataRegion = NDS.ARM9Regions[addr>>14];
         if ((NDS.ARM9Timestamp <= WBReleaseTS) && (DataRegion == WBLastRegion)) // check write buffer
             NDS.ARM9Timestamp += 1<<NDS.ARM9ClockShift;
-
-        if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-            WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-        *val = BusRead16(addr);
+            
+        QueueFunction(&ARMv5::DRead16_5);
     }
 }
 
+void ARMv5::DRead16_5()
+{
+    u8 reg = __builtin_ctz(LDRRegs);
+    u32 addr = FetchAddr[reg];
+    u32 dummy; u32* val = (LDRFailedRegs & (1<<reg)) ? &dummy : &R[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr >> 14][0];
+    DataCycles = 3<<NDS.ARM9ClockShift;
+
+    if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    *val = BusRead16(addr);
+}
+
 void ARMv5::DCacheFin32()
 {
     u8 reg = __builtin_ctz(LDRRegs);
@@ -2639,19 +2675,34 @@ void ARMv5::DRead32_4()
         MRTrack.Type = MainRAMType::Fetch;
         MRTrack.Var = MR32;
         MRTrack.Progress = reg;
+
+        LDRRegs &= ~1<<reg;
     }
     else
     {
-        NDS.ARM9Timestamp += MemTimings[addr >> 14][1];
-        DataCycles = 3<<NDS.ARM9ClockShift;
         DataRegion = NDS.ARM9Regions[addr>>14];
         if ((NDS.ARM9Timestamp <= WBReleaseTS) && (DataRegion == WBLastRegion)) // check write buffer
             NDS.ARM9Timestamp += 1<<NDS.ARM9ClockShift;
-    
-        if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-            WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-        *val = BusRead32(addr);
+
+        QueueFunction(&ARMv5::DRead32_5);
     }
+}
+
+void ARMv5::DRead32_5()
+{
+    u8 reg = __builtin_ctz(LDRRegs);
+    u32 addr = FetchAddr[reg];
+    u32 dummy; u32* val = (LDRFailedRegs & (1<<reg)) ? &dummy : &R[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr >> 14][1];
+    DataCycles = 3<<NDS.ARM9ClockShift;
+    
+    if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    *val = BusRead32(addr);
     LDRRegs &= ~1<<reg;
 }
 
@@ -2733,7 +2784,7 @@ void ARMv5::DRead32S_3()
         WriteBufferDrain();
     else
         WriteBufferCheck<1>();
-        
+
     QueueFunction(&ARMv5::DRead32S_4);
 }
 
@@ -2751,18 +2802,16 @@ void ARMv5::DRead32S_4()
             MRTrack.Type = MainRAMType::Fetch;
             MRTrack.Var = MR32 | MRSequential;
             MRTrack.Progress = reg;
+
+            LDRRegs &= ~1<<reg;
         }
         else
         {
-            NDS.ARM9Timestamp += MemTimings[addr>>14][2];
-            DataCycles = MemTimings[addr>>14][2];
             DataRegion = NDS.ARM9Regions[addr>>14];
             if ((NDS.ARM9Timestamp <= WBReleaseTS) && (DataRegion == WBLastRegion)) // check write buffer
                 NDS.ARM9Timestamp += 1<<NDS.ARM9ClockShift;
-    
-            if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-                WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-            *val = BusRead32(addr);
+
+            QueueFunction(&ARMv5::DRead32S_5A);
         }
     }
     else // ns
@@ -2774,20 +2823,53 @@ void ARMv5::DRead32S_4()
             MRTrack.Type = MainRAMType::Fetch;
             MRTrack.Var = MR32;
             MRTrack.Progress = reg;
+
+            LDRRegs &= ~1<<reg;
         }
         else
         {
-            NDS.ARM9Timestamp += MemTimings[addr>>14][1];
-            DataCycles = 3<<NDS.ARM9ClockShift;
             DataRegion = NDS.ARM9Regions[addr>>14];
             if ((NDS.ARM9Timestamp <= WBReleaseTS) && (DataRegion == WBLastRegion)) // check write buffer
                 NDS.ARM9Timestamp += 1<<NDS.ARM9ClockShift;
-    
-            if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-                WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-            *val = BusRead32(addr);
+
+            QueueFunction(&ARMv5::DRead32S_5B);
         }
     }
+}
+
+void ARMv5::DRead32S_5A()
+{
+    u8 reg = __builtin_ctz(LDRRegs);
+    u32 addr = FetchAddr[reg];
+    u32 dummy; u32* val = (LDRFailedRegs & (1<<reg)) ? &dummy : &R[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr>>14][2];
+    DataCycles = MemTimings[addr>>14][2];
+    
+    if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    *val = BusRead32(addr);
+    LDRRegs &= ~1<<reg;
+}
+
+void ARMv5::DRead32S_5B()
+{
+    u8 reg = __builtin_ctz(LDRRegs);
+    u32 addr = FetchAddr[reg];
+    u32 dummy; u32* val = (LDRFailedRegs & (1<<reg)) ? &dummy : &R[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr>>14][1];
+    DataCycles = 3<<NDS.ARM9ClockShift;
+    
+    if (WBTimestamp < ((NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp - (3<<NDS.ARM9ClockShift) + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    *val = BusRead32(addr);
     LDRRegs &= ~1<<reg;
 }
 
@@ -2898,16 +2980,28 @@ void ARMv5::DWrite8_4()
     }
     else
     {
-        NDS.ARM9Timestamp += MemTimings[addr >> 14][0];
-        DataCycles = 3<<NDS.ARM9ClockShift;
-        DataRegion = NDS.ARM9Regions[addr>>14];
-        
-        if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-            WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-        BusWrite8(addr, val);
+        QueueFunction(&ARMv5::DWrite8_5);
     }
 }
 
+void ARMv5::DWrite8_5()
+{
+    u8 reg = __builtin_ctz(STRRegs);
+    u32 addr = FetchAddr[reg];
+    u8 val = STRVal[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr >> 14][0];
+    DataCycles = 3<<NDS.ARM9ClockShift;
+    DataRegion = NDS.ARM9Regions[addr>>14];
+        
+    if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    BusWrite8(addr, val);
+}
+
 bool ARMv5::DataWrite16(u32 addr, u16 val, u8 reg)
 {
     // Data Aborts
@@ -3017,16 +3111,28 @@ void ARMv5::DWrite16_4()
     }
     else
     {
-        NDS.ARM9Timestamp += MemTimings[addr >> 14][0];
-        DataCycles = 3<<NDS.ARM9ClockShift;
-        DataRegion = NDS.ARM9Regions[addr>>14];
-        
-        if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-            WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-        BusWrite16(addr, val);
+        QueueFunction(&ARMv5::DWrite16_5);
     }
 }
 
+void ARMv5::DWrite16_5()
+{
+    u8 reg = __builtin_ctz(STRRegs);
+    u32 addr = FetchAddr[reg];
+    u16 val = STRVal[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr >> 14][0];
+    DataCycles = 3<<NDS.ARM9ClockShift;
+    DataRegion = NDS.ARM9Regions[addr>>14];
+        
+    if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    BusWrite16(addr, val);
+}
+
 bool ARMv5::DataWrite32(u32 addr, u32 val, u8 reg)
 {
     // Data Aborts
@@ -3139,17 +3245,31 @@ void ARMv5::DWrite32_4()
         MRTrack.Type = MainRAMType::Fetch;
         MRTrack.Var = MRWrite | MR32;
         MRTrack.Progress = reg;
+        
+        STRRegs &= ~1<<reg;
     }
     else
     {
-        NDS.ARM9Timestamp += MemTimings[addr >> 14][1];
-        DataCycles = 3<<NDS.ARM9ClockShift;
-        DataRegion = NDS.ARM9Regions[addr>>14];
-        
-        if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-            WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-        BusWrite32(addr, val);
+        QueueFunction(&ARMv5::DWrite32_5);
     }
+}
+
+void ARMv5::DWrite32_5()
+{
+    u8 reg = __builtin_ctz(STRRegs);
+    u32 addr = FetchAddr[reg];
+    u32 val = STRVal[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr >> 14][1];
+    DataCycles = 3<<NDS.ARM9ClockShift;
+    DataRegion = NDS.ARM9Regions[addr>>14];
+        
+    if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    BusWrite32(addr, val);
     STRRegs &= ~1<<reg;
 }
 
@@ -3255,15 +3375,12 @@ void ARMv5::DWrite32S_4()
             MRTrack.Type = MainRAMType::Fetch;
             MRTrack.Var = MRWrite | MR32 | MRSequential;
             MRTrack.Progress = reg;
+
+            STRRegs &= ~1<<reg;
         }
         else
         {
-            NDS.ARM9Timestamp += MemTimings[addr>>14][2];
-            DataRegion = NDS.ARM9Regions[addr>>14];
-        
-            if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-                WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-            BusWrite32(addr, val);
+            QueueFunction(&ARMv5::DWrite32S_5A);
         }
     }
     else // ns
@@ -3275,18 +3392,50 @@ void ARMv5::DWrite32S_4()
             MRTrack.Type = MainRAMType::Fetch;
             MRTrack.Var = MRWrite | MR32;
             MRTrack.Progress = reg;
+
+            STRRegs &= ~1<<reg;
         }
         else
         {
-            NDS.ARM9Timestamp += MemTimings[addr>>14][1];
-            DataCycles = 3 << NDS.ARM9ClockShift;
-            DataRegion = NDS.ARM9Regions[addr>>14];
-        
-            if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
-                WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
-            BusWrite32(addr, val);
+            QueueFunction(&ARMv5::DWrite32S_5B);
         }
     }
+}
+
+void ARMv5::DWrite32S_5A()
+{
+    u8 reg = __builtin_ctz(STRRegs);
+    u32 addr = FetchAddr[reg];
+    u32 val = STRVal[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr>>14][2];
+    DataRegion = NDS.ARM9Regions[addr>>14];
+
+    if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    BusWrite32(addr, val);
+    STRRegs &= ~1<<reg;
+}
+
+void ARMv5::DWrite32S_5B()
+{
+    u8 reg = __builtin_ctz(STRRegs);
+    u32 addr = FetchAddr[reg];
+    u32 val = STRVal[reg];
+
+    if (NDS.ARM9Timestamp < NDS.DMA9Timestamp) NDS.ARM9Timestamp = NDS.DMA9Timestamp;
+
+    NDS.ARM9Timestamp += MemTimings[addr>>14][1];
+    DataCycles = 3 << NDS.ARM9ClockShift;
+    DataRegion = NDS.ARM9Regions[addr>>14];
+
+    if (WBTimestamp < ((NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1)))
+        WBTimestamp = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+
+    BusWrite32(addr, val);
     STRRegs &= ~1<<reg;
 }
 
diff --git a/src/DMA.cpp b/src/DMA.cpp
index 5c8aaaf2..c5319c2e 100644
--- a/src/DMA.cpp
+++ b/src/DMA.cpp
@@ -146,7 +146,10 @@ void DMA::WriteCnt(u32 val)
             StartMode = ((Cnt >> 28) & 0x3) | 0x10;
 
         if ((StartMode & 0x7) == 0)
-            Start();
+        {
+            NDS.DMAsQueued[NDS.DMAQueuePtr++] = (CPU*4)+Num;
+            if (!(NDS.SchedListMask & (1<<Event_DMA))) NDS.ScheduleEvent(Event_DMA, false, 1, 0, 0);
+        }
         else if (StartMode == 0x07)
             NDS.GPU.GPU3D.CheckFIFODMA();
 
@@ -212,6 +215,18 @@ void DMA::Start()
     InProgress = true;
     NDS.StopCPU(CPU, 1<<Num);
 
+    if (CPU == 0)
+    {
+        u64 ts;
+        /*if (StartMode == 0x00)
+        {
+            ts = (NDS.ARM9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+        }
+        else*/ ts = NDS.SysTimestamp << NDS.ARM9ClockShift;
+
+        if (NDS.DMA9Timestamp < ts) NDS.DMA9Timestamp = ts;
+    }
+
     if (Num == 0) NDS.DMAs[(CPU*4)+1].ResetBurst();
     if (Num <= 1) NDS.DMAs[(CPU*4)+2].ResetBurst();
     if (Num <= 2) NDS.DMAs[(CPU*4)+3].ResetBurst();
@@ -587,8 +602,8 @@ u32 DMA::UnitTimings7_32(int burststart)
 
 void DMA::Run9()
 {
-    NDS.DMA9Timestamp = std::max(NDS.DMA9Timestamp, NDS.ARM9Timestamp);
-    NDS.DMA9Timestamp = (NDS.DMA9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
+    //NDS.DMA9Timestamp = std::max(NDS.DMA9Timestamp, NDS.SysTimestamp << NDS.ARM9ClockShift);
+    //NDS.DMA9Timestamp = (NDS.DMA9Timestamp + ((1<<NDS.ARM9ClockShift)-1)) & ~((1<<NDS.ARM9ClockShift)-1);
 
     if (NDS.DMA9Timestamp-1 >= NDS.ARM9Target) return;
 
diff --git a/src/DMA.h b/src/DMA.h
index 3e9ebd50..4bad6110 100644
--- a/src/DMA.h
+++ b/src/DMA.h
@@ -75,7 +75,7 @@ public:
 
     void ResetBurst()
     {
-        if (Running > 0) Running = (CPU ? 2 : 3);
+        if (Running > 0) Running = 3;
     }
 
     u32 SrcAddr {};
diff --git a/src/NDS.cpp b/src/NDS.cpp
index 91aee9d4..90008e04 100644
--- a/src/NDS.cpp
+++ b/src/NDS.cpp
@@ -124,6 +124,7 @@ NDS::NDS(NDSArgs&& args, int type, void* userdata) noexcept :
 {
     RegisterEventFuncs(Event_Div, this, {MakeEventThunk(NDS, DivDone)});
     RegisterEventFuncs(Event_Sqrt, this, {MakeEventThunk(NDS, SqrtDone)});
+    RegisterEventFuncs(Event_DMA, this, {MakeEventThunk(NDS, QueueDMAs)});
 
     MainRAM = JIT.Memory.GetMainRAM();
     SharedWRAM = JIT.Memory.GetSharedWRAM();
@@ -134,6 +135,7 @@ NDS::~NDS() noexcept
 {
     UnregisterEventFuncs(Event_Div);
     UnregisterEventFuncs(Event_Sqrt);
+    UnregisterEventFuncs(Event_DMA);
     // The destructor for each component is automatically called by the compiler
 }
 
@@ -548,6 +550,9 @@ void NDS::Reset()
     KeyCnt[1] = 0;
     RCnt = 0;
 
+    memset(DMAsQueued, 0, sizeof(DMAsQueued));
+    DMAQueuePtr = 0;
+
     GPU.Reset();
     NDSCartSlot.Reset();
     GBACartSlot.Reset();
@@ -1818,8 +1823,8 @@ u32 NDS::RunFrame()
                     }
                 }
                 
-                NDSCartSlot.ROMPrepareData();
                 RunSystem(target);
+                NDSCartSlot.ROMPrepareData();
 
                 if (CPUStop & CPUStop_Sleep)
                 {
@@ -2526,6 +2531,15 @@ void NDS::StopDMAs(u32 cpu, u32 mode)
     DMAs[cpu+3].StopIfNeeded(mode);
 }
 
+void NDS::QueueDMAs(u32 param)
+{
+    DMAs[DMAsQueued[0]].Start();
+    for(int i = 0; i < 7; i++) DMAsQueued[i] = DMAsQueued[i+1];
+    DMAQueuePtr--;
+
+    if (DMAQueuePtr != 0) ScheduleEvent(Event_DMA, false, 1, 0, 0);
+}
+
 
 
 void NDS::DivDone(u32 param)
diff --git a/src/NDS.h b/src/NDS.h
index e933d951..722f91b4 100644
--- a/src/NDS.h
+++ b/src/NDS.h
@@ -64,6 +64,7 @@ enum
     Event_SPITransfer,
     Event_Div,
     Event_Sqrt,
+    Event_DMA,
 
     // DSi
     Event_DSi_SDMMCTransfer,
@@ -243,7 +244,8 @@ public: // TODO: Encapsulate the rest of these members
 
     int ConsoleType;
     int CurCPU;
-
+    
+    u32 SchedListMask;
     SchedEvent SchedList[Event_MAX] {};
     u8 ARM9MemTimings[0x40000][8];
     u32 ARM9Regions[0x40000];
@@ -259,6 +261,7 @@ public: // TODO: Encapsulate the rest of these members
     u64 ARM7Timestamp, ARM7Target;
     u64 MainRAMTimestamp, MainRAMBurstStart;
     u64 A9ContentionTS; bool ConTSLock;
+    u64 SysTimestamp;
     u32 ARM9ClockShift;
 
     u32 IME[2];
@@ -277,6 +280,8 @@ public: // TODO: Encapsulate the rest of these members
     alignas(u32) u8 ROMSeed1[2*8];
 
     u32 DMAReadHold[2];
+    u8 DMAsQueued[8];
+    u8 DMAQueuePtr;
     bool MainRAMBork; // if a main ram read burst starts in the last 6 bytes of a 32 byte block, and then crosses the 32 byte boundary, the burst forcibly restarts
     bool MainRAMLastAccess; // 0 == ARM9 | 1 == ARM7
     bool DMALastWasMainRAM;
@@ -506,8 +511,6 @@ public: // TODO: Encapsulate the rest of these members
 
 private:
     void InitTimings();
-    u32 SchedListMask;
-    u64 SysTimestamp;
     u8 WRAMCnt;
     u8 PostFlag9;
     u8 PostFlag7;
@@ -542,6 +545,7 @@ private:
     void HandleTimerOverflow(u32 tid);
     u16 TimerGetCounter(u32 timer);
     void TimerStart(u32 id, u16 cnt);
+    void QueueDMAs(u32 param);
     void StartDiv();
     void DivDone(u32 param);
     void SqrtDone(u32 param);
diff --git a/src/NDSCart.cpp b/src/NDSCart.cpp
index ef24cdb1..25458914 100644
--- a/src/NDSCart.cpp
+++ b/src/NDSCart.cpp
@@ -1982,10 +1982,6 @@ void NDSCartSlot::AdvanceROMTransfer() noexcept
 u32 NDSCartSlot::ReadROMData() noexcept
 {
     if (ROMCnt & (1<<30)) return 0;
-    
-    u64 curts;
-    if (NDS.ExMemCnt[0] & (1<<11)) curts = NDS.ARM7Timestamp;
-    else                           curts = (std::max(NDS.ARM9Timestamp, NDS.DMA9Timestamp) + ((1<<NDS.ARM9ClockShift)-1)) >> NDS.ARM9ClockShift;
 
     ROMPrepareData();
 
@@ -2000,11 +1996,7 @@ u32 NDSCartSlot::ReadROMData() noexcept
 void NDSCartSlot::WriteROMData(u32 val) noexcept
 {
     if (!(ROMCnt & (1<<30))) return;
-    
-    u64 curts;
-    if (NDS.ExMemCnt[0] & (1<<11)) curts = NDS.ARM7Timestamp;
-    else                           curts = (std::max(NDS.ARM9Timestamp, NDS.DMA9Timestamp) + ((1<<NDS.ARM9ClockShift)-1)) >> NDS.ARM9ClockShift;
-    
+
     ROMPrepareData();
 
     ROMData = val;