Peek and Poke 16 fix

EremusOne · Jul 24, 2023 · e6690c9 · e6690c9
1 parent f4e944b
commit e6690c9
Showing 1 changed file with 40 additions and 17 deletions.
diff --git a/src/CPU.cpp b/src/CPU.cpp
@@ -218,31 +218,54 @@ void IRAM_ATTR Z80Ops::poke8(uint16_t address, uint8_t value) {
 /* Read/Write word from/to RAM */
 uint16_t IRAM_ATTR Z80Ops::peek16(uint16_t address) {
 
-    uint8_t page = address >> 14;
+    // Check if address is between two different pages
+    if ((address >> 14) == ((address + 1) >> 14)) {
+
+        uint8_t page = address >> 14;
 
-    if (MemESP::ramContended[page]) {
-        VIDEO::Draw(3, true);
-        VIDEO::Draw(3, true);            
-    } else
-        VIDEO::Draw(6, false);
+        if (MemESP::ramContended[page]) {
+            VIDEO::Draw(3, true);
+            VIDEO::Draw(3, true);            
+        } else
+            VIDEO::Draw(6, false);
 
-    return ((MemESP::ramCurrent[page][(address & 0x3fff) + 1] << 8) | MemESP::ramCurrent[page][address & 0x3fff]);
+        return ((MemESP::ramCurrent[page][(address & 0x3fff) + 1] << 8) | MemESP::ramCurrent[page][address & 0x3fff]);
+
+    } else {
+
+        // Order matters, first read lsb, then read msb, don't "optimize"
+        uint8_t lsb = Z80Ops::peek8(address);
+        uint8_t msb = Z80Ops::peek8(address + 1);
+        return (msb << 8) | lsb;
+
+    }
 
 }
 
 void IRAM_ATTR Z80Ops::poke16(uint16_t address, RegisterPair word) {
 
-    uint8_t page = address >> 14;
+    // Check if address is between two different pages
+    if ((address >> 14) == ((address + 1) >> 14)) {
+
+        uint8_t page = address >> 14;
 
-    if (MemESP::ramContended[page]) {
-        VIDEO::Draw(3, true);
-        VIDEO::Draw(3, true);            
-    } else
-        VIDEO::Draw(6, false);
-
-    if (page != 0) {
-        MemESP::ramCurrent[page][address & 0x3fff] = word.byte8.lo;
-        MemESP::ramCurrent[page][(address & 0x3fff) + 1] = word.byte8.hi;
+        if (MemESP::ramContended[page]) {
+            VIDEO::Draw(3, true);
+            VIDEO::Draw(3, true);            
+        } else
+            VIDEO::Draw(6, false);
+
+        if (page != 0) {
+            MemESP::ramCurrent[page][address & 0x3fff] = word.byte8.lo;
+            MemESP::ramCurrent[page][(address & 0x3fff) + 1] = word.byte8.hi;
+        }
+
+    } else {
+
+        // Order matters, first write lsb, then write msb, don't "optimize"
+        Z80Ops::poke8(address, word.byte8.lo);
+        Z80Ops::poke8(address + 1, word.byte8.hi);
+
     }
 
 }