From 82578cd914aaa8a0fbdb16aa93b52fcba2d8b676 Mon Sep 17 00:00:00 2001
From: CocoSimone <simococo03@gmail.com>
Date: Tue, 20 Sep 2022 23:01:18 +0200
Subject: [PATCH] Fix various memory things

---
 src/n64/core/Cpu.cpp                          |   2 +-
 src/n64/core/Mem.cpp                          | 341 ++++++++++++------
 src/n64/core/Mem.hpp                          |  22 +-
 src/n64/core/cpu/instructions.cpp             |  54 +--
 .../core/cpu/registers/cop1instructions.cpp   |   8 +-
 src/n64/core/mmio/AI.cpp                      |   2 +-
 6 files changed, 291 insertions(+), 138 deletions(-)

diff --git a/src/n64/core/Cpu.cpp b/src/n64/core/Cpu.cpp
index df24faf9..96b68366 100644
--- a/src/n64/core/Cpu.cpp
+++ b/src/n64/core/Cpu.cpp
@@ -94,7 +94,7 @@ void Cpu::Step(Mem& mem) {
   CheckCompareInterrupt(mem.mmio.mi, regs);
   HandleInterrupt(regs);
 
-  u32 instruction = mem.Read<u32>(regs, regs.pc, regs.pc);
+  u32 instruction = mem.Read32(regs, regs.pc, regs.pc);
 
   regs.oldPC = regs.pc;
   regs.pc = regs.nextPC;
diff --git a/src/n64/core/Mem.cpp b/src/n64/core/Mem.cpp
index cc158c65..7e851101 100644
--- a/src/n64/core/Mem.cpp
+++ b/src/n64/core/Mem.cpp
@@ -63,8 +63,8 @@ bool MapVAddr(Registers& regs, TLBAccessType accessType, u32 vaddr, u32& paddr)
 template bool MapVAddr<true>(Registers& regs, TLBAccessType accessType, u32 vaddr, u32& paddr);
 template bool MapVAddr<false>(Registers& regs, TLBAccessType accessType, u32 vaddr, u32& paddr);
 
-template <class T, bool tlb>
-T Mem::Read(Registers& regs, u32 vaddr, s64 pc) {
+template <bool tlb>
+u8 Mem::Read8(n64::Registers &regs, u32 vaddr, s64 pc) {
   u32 paddr = vaddr;
   if(!MapVAddr<tlb>(regs, LOAD, vaddr, paddr)) {
     HandleTLBException(regs, vaddr);
@@ -73,76 +73,135 @@ T Mem::Read(Registers& regs, u32 vaddr, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      if constexpr (sizeof(T) == 1) {
-        return util::ReadAccess<T>(mmio.rdp.dram.data(), BYTE_ADDRESS(paddr) & RDRAM_DSIZE);
-      } else if constexpr (sizeof(T) == 2) {
-        return util::ReadAccess<T>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr) & RDRAM_DSIZE);
-      } else {
-        return util::ReadAccess<T>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE);
-      }
+      return mmio.rdp.dram[BYTE_ADDRESS(paddr) & RDRAM_DSIZE];
     case 0x04000000 ... 0x0403FFFF:
-      if constexpr (sizeof(T) == 1) {
-        return mmio.rsp.ReadByte(paddr, paddr & 0x1000);
-      } else if constexpr (sizeof(T) == 2) {
-        return mmio.rsp.ReadHalf(paddr, paddr & 0x1000);
-      } else if constexpr (sizeof(T) == 4) {
-        return mmio.rsp.ReadWord(paddr, paddr & 0x1000);
-      } else {
-        return mmio.rsp.ReadDword(paddr, paddr & 0x1000);
-      }
+      if(paddr & 0x1000)
+        return mmio.rsp.imem[BYTE_ADDRESS(paddr) & IMEM_DSIZE];
+      else
+        return mmio.rsp.dmem[BYTE_ADDRESS(paddr) & DMEM_DSIZE];
     case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
-    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF: return mmio.Read(paddr);
+    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF:
+      return mmio.Read(paddr);
     case 0x10000000 ... 0x1FBFFFFF:
-      if constexpr (sizeof(T) == 1) {
-        return util::ReadAccess<T>(cart.data(), BYTE_ADDRESS(paddr) & romMask);
-      } else if constexpr (sizeof(T) == 2) {
-        return util::ReadAccess<T>(cart.data(), HALF_ADDRESS(paddr) & romMask);
-      } else {
-        return util::ReadAccess<T>(cart.data(), paddr & romMask);
-      }
+      return cart[BYTE_ADDRESS(paddr) & romMask];
     case 0x1FC00000 ... 0x1FC007BF:
-      if constexpr (sizeof(T) == 1) {
-        return util::ReadAccess<T>(pifBootrom, BYTE_ADDRESS(paddr) & PIF_BOOTROM_DSIZE);
-      } else if constexpr (sizeof(T) == 2) {
-        return util::ReadAccess<T>(pifBootrom, HALF_ADDRESS(paddr) & PIF_BOOTROM_DSIZE);
-      } else {
-        return util::ReadAccess<T>(pifBootrom, paddr & PIF_BOOTROM_DSIZE);
-      }
+      return pifBootrom[BYTE_ADDRESS(paddr) & PIF_BOOTROM_DSIZE];
     case 0x1FC007C0 ... 0x1FC007FF:
-      if constexpr (sizeof(T) == 1) {
-        return util::ReadAccess<T>(pifRam, BYTE_ADDRESS(paddr) & PIF_RAM_DSIZE);
-      } else if constexpr (sizeof(T) == 2) {
-        return util::ReadAccess<T>(pifRam, HALF_ADDRESS(paddr) & PIF_RAM_DSIZE);
-      } else {
-        return util::ReadAccess<T>(pifRam, paddr & PIF_RAM_DSIZE);
-      }
+      return pifRam[paddr & PIF_RAM_DSIZE];
     case 0x00800000 ... 0x03FFFFFF: case 0x04200000 ... 0x042FFFFF:
     case 0x04900000 ... 0x07FFFFFF: case 0x08000000 ... 0x0FFFFFFF:
     case 0x80000000 ... 0xFFFFFFFF: case 0x1FC00800 ... 0x7FFFFFFF: return 0;
-    default: util::panic("Unimplemented {}-bit read at address {:08X} (PC = {:016X})\n", sizeof(T) * 8, paddr, (u64)regs.pc);
+    default: util::panic("Unimplemented 8-bit read at address {:08X} (PC = {:016X})\n", paddr, (u64)regs.pc);
   }
-  return 0;
 }
 
-template u8 Mem::Read<u8>(Registers& regs, u32 vaddr, s64 pc);
-template u16 Mem::Read<u16>(Registers& regs, u32 vaddr, s64 pc);
-template u32 Mem::Read<u32>(Registers& regs, u32 vaddr, s64 pc);
-template u64 Mem::Read<u64>(Registers& regs, u32 vaddr, s64 pc);
-template u8 Mem::Read<u8, false>(Registers& regs, u32 vaddr, s64 pc);
-template u16 Mem::Read<u16, false>(Registers& regs, u32 vaddr, s64 pc);
-template u32 Mem::Read<u32, false>(Registers& regs, u32 vaddr, s64 pc);
-template u64 Mem::Read<u64, false>(Registers& regs, u32 vaddr, s64 pc);
-template s8 Mem::Read<s8>(Registers& regs, u32 vaddr, s64 pc);
-template s16 Mem::Read<s16>(Registers& regs, u32 vaddr, s64 pc);
-template s32 Mem::Read<s32>(Registers& regs, u32 vaddr, s64 pc);
-template s64 Mem::Read<s64>(Registers& regs, u32 vaddr, s64 pc);
-template s8 Mem::Read<s8, false>(Registers& regs, u32 vaddr, s64 pc);
-template s16 Mem::Read<s16, false>(Registers& regs, u32 vaddr, s64 pc);
-template s32 Mem::Read<s32, false>(Registers& regs, u32 vaddr, s64 pc);
-template s64 Mem::Read<s64, false>(Registers& regs, u32 vaddr, s64 pc);
+template <bool tlb>
+u16 Mem::Read16(n64::Registers &regs, u32 vaddr, s64 pc) {
+  u32 paddr = vaddr;
+  if(!MapVAddr<tlb>(regs, LOAD, vaddr, paddr)) {
+    HandleTLBException(regs, vaddr);
+    FireException(regs, GetTLBExceptionCode(regs.cop0.tlbError, LOAD), 0, pc);
+  }
 
-template <class T, bool tlb>
-void Mem::Write(Registers& regs, u32 vaddr, T val, s64 pc) {
+  switch(paddr) {
+    case 0x00000000 ... 0x007FFFFF:
+      return util::ReadAccess<u16>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr) & RDRAM_DSIZE);
+    case 0x04000000 ... 0x0403FFFF:
+      if(paddr & 0x1000)
+        return util::ReadAccess<u16>(mmio.rsp.imem, HALF_ADDRESS(paddr) & IMEM_DSIZE);
+      else
+        return util::ReadAccess<u16>(mmio.rsp.dmem, HALF_ADDRESS(paddr) & DMEM_DSIZE);
+    case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
+    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF:
+      return mmio.Read(paddr);
+    case 0x10000000 ... 0x1FBFFFFF:
+      return util::ReadAccess<u16>(cart.data(), HALF_ADDRESS(paddr) & romMask);
+    case 0x1FC00000 ... 0x1FC007BF:
+      return util::ReadAccess<u16>(pifBootrom, HALF_ADDRESS(paddr) & PIF_BOOTROM_DSIZE);
+    case 0x1FC007C0 ... 0x1FC007FF:
+      return be16toh(util::ReadAccess<u16>(pifRam, paddr & PIF_RAM_DSIZE));
+    case 0x00800000 ... 0x03FFFFFF: case 0x04200000 ... 0x042FFFFF:
+    case 0x04900000 ... 0x07FFFFFF: case 0x08000000 ... 0x0FFFFFFF:
+    case 0x80000000 ... 0xFFFFFFFF: case 0x1FC00800 ... 0x7FFFFFFF: return 0;
+    default: util::panic("Unimplemented 16-bit read at address {:08X} (PC = {:016X})\n", paddr, (u64)regs.pc);
+  }
+}
+
+template <bool tlb>
+u32 Mem::Read32(n64::Registers &regs, u32 vaddr, s64 pc) {
+  u32 paddr = vaddr;
+  if(!MapVAddr<tlb>(regs, LOAD, vaddr, paddr)) {
+    HandleTLBException(regs, vaddr);
+    FireException(regs, GetTLBExceptionCode(regs.cop0.tlbError, LOAD), 0, pc);
+  }
+
+  switch(paddr) {
+    case 0x00000000 ... 0x007FFFFF:
+      return util::ReadAccess<u32>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE);
+    case 0x04000000 ... 0x0403FFFF:
+      if(paddr & 0x1000)
+        return util::ReadAccess<u32>(mmio.rsp.imem, paddr & IMEM_DSIZE);
+      else
+        return util::ReadAccess<u32>(mmio.rsp.dmem, paddr & DMEM_DSIZE);
+    case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
+    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF:
+      return mmio.Read(paddr);
+    case 0x10000000 ... 0x1FBFFFFF:
+      return util::ReadAccess<u32>(cart.data(), paddr & romMask);
+    case 0x1FC00000 ... 0x1FC007BF:
+      return util::ReadAccess<u32>(pifBootrom, paddr & PIF_BOOTROM_DSIZE);
+    case 0x1FC007C0 ... 0x1FC007FF:
+      return be32toh(util::ReadAccess<u32>(pifRam, paddr & PIF_RAM_DSIZE));
+    case 0x00800000 ... 0x03FFFFFF: case 0x04200000 ... 0x042FFFFF:
+    case 0x04900000 ... 0x07FFFFFF: case 0x08000000 ... 0x0FFFFFFF:
+    case 0x80000000 ... 0xFFFFFFFF: case 0x1FC00800 ... 0x7FFFFFFF: return 0;
+    default: util::panic("Unimplemented 32-bit read at address {:08X} (PC = {:016X})\n", paddr, (u64)regs.pc);
+  }
+}
+
+template <bool tlb>
+u64 Mem::Read64(n64::Registers &regs, u32 vaddr, s64 pc) {
+  u32 paddr = vaddr;
+  if(!MapVAddr<tlb>(regs, LOAD, vaddr, paddr)) {
+    HandleTLBException(regs, vaddr);
+    FireException(regs, GetTLBExceptionCode(regs.cop0.tlbError, LOAD), 0, pc);
+  }
+
+  switch(paddr) {
+    case 0x00000000 ... 0x007FFFFF:
+      return util::ReadAccess<u64>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE);
+    case 0x04000000 ... 0x0403FFFF:
+      if(paddr & 0x1000)
+        return util::ReadAccess<u64>(mmio.rsp.imem, paddr & IMEM_DSIZE);
+      else
+        return util::ReadAccess<u64>(mmio.rsp.dmem, paddr & DMEM_DSIZE);
+    case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
+    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF:
+      return mmio.Read(paddr);
+    case 0x10000000 ... 0x1FBFFFFF:
+      return util::ReadAccess<u64>(cart.data(), paddr & romMask);
+    case 0x1FC00000 ... 0x1FC007BF:
+      return util::ReadAccess<u64>(pifBootrom, paddr & PIF_BOOTROM_DSIZE);
+    case 0x1FC007C0 ... 0x1FC007FF:
+      return be64toh(util::ReadAccess<u64>(pifRam, paddr & PIF_RAM_DSIZE));
+    case 0x00800000 ... 0x03FFFFFF: case 0x04200000 ... 0x042FFFFF:
+    case 0x04900000 ... 0x07FFFFFF: case 0x08000000 ... 0x0FFFFFFF:
+    case 0x80000000 ... 0xFFFFFFFF: case 0x1FC00800 ... 0x7FFFFFFF: return 0;
+    default: util::panic("Unimplemented 32-bit read at address {:08X} (PC = {:016X})\n", paddr, (u64)regs.pc);
+  }
+}
+
+template u8 Mem::Read8<false>(n64::Registers &regs, u32 vaddr, s64 pc);
+template u8 Mem::Read8<true>(n64::Registers &regs, u32 vaddr, s64 pc);
+template u16 Mem::Read16<false>(n64::Registers &regs, u32 vaddr, s64 pc);
+template u16 Mem::Read16<true>(n64::Registers &regs, u32 vaddr, s64 pc);
+template u32 Mem::Read32<false>(n64::Registers &regs, u32 vaddr, s64 pc);
+template u32 Mem::Read32<true>(n64::Registers &regs, u32 vaddr, s64 pc);
+template u64 Mem::Read64<false>(n64::Registers &regs, u32 vaddr, s64 pc);
+template u64 Mem::Read64<true>(n64::Registers &regs, u32 vaddr, s64 pc);
+
+template <bool tlb>
+void Mem::Write8(Registers& regs, u32 vaddr, u32 val, s64 pc) {
   u32 paddr = vaddr;
   if(!MapVAddr<tlb>(regs, STORE, vaddr, paddr)) {
     HandleTLBException(regs, vaddr);
@@ -151,24 +210,85 @@ void Mem::Write(Registers& regs, u32 vaddr, T val, s64 pc) {
 
   switch(paddr) {
     case 0x00000000 ... 0x007FFFFF:
-      if constexpr (sizeof(T) == 1) {
-        util::WriteAccess<T>(mmio.rdp.dram.data(), BYTE_ADDRESS(paddr) & RDRAM_DSIZE, val);
-      } else if constexpr (sizeof(T) == 2) {
-        util::WriteAccess<T>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr) & RDRAM_DSIZE, val);
-      } else {
-        util::WriteAccess<T>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE, val);
-      }
+      mmio.rdp.dram[BYTE_ADDRESS(paddr) & RDRAM_DSIZE] = val;
       break;
     case 0x04000000 ... 0x0403FFFF:
-      if constexpr (sizeof(T) == 1) {
-        mmio.rsp.WriteByte(paddr, val, paddr & 0x1000);
-      } else if constexpr (sizeof(T) == 2) {
-        mmio.rsp.WriteHalf(paddr, val, paddr & 0x1000);
-      } else if constexpr (sizeof(T) == 4) {
-        mmio.rsp.WriteWord(paddr, val, paddr & 0x1000);
-      } else {
-        mmio.rsp.WriteDword(paddr, val, paddr & 0x1000);
-      }
+      val = val << (8 * (3 - (paddr & 3)));
+      paddr = (paddr & DMEM_DSIZE) & ~3;
+      if(paddr & 0x1000)
+        util::WriteAccess<u32>(mmio.rsp.imem, paddr & IMEM_DSIZE, val);
+      else
+        util::WriteAccess<u32>(mmio.rsp.dmem, paddr & DMEM_DSIZE, val);
+      break;
+    case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
+    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF: mmio.Write(*this, regs, paddr, val); break;
+    case 0x10000000 ... 0x13FFFFFF: break;
+    case 0x1FC007C0 ... 0x1FC007FF:
+      val = val << (8 * (3 - (paddr & 3)));
+      paddr = (paddr & PIF_RAM_DSIZE) & ~3;
+      util::WriteAccess<u32>(pifRam, paddr & PIF_RAM_DSIZE, htobe32(val));
+      ProcessPIFCommands(pifRam, mmio.si.controller, *this);
+      break;
+    case 0x00800000 ... 0x03FFFFFF: case 0x04200000 ... 0x042FFFFF:
+    case 0x08000000 ... 0x0FFFFFFF: case 0x04900000 ... 0x07FFFFFF:
+    case 0x1FC00800 ... 0x7FFFFFFF: case 0x80000000 ... 0xFFFFFFFF: break;
+    default: util::panic("Unimplemented 8-bit write at address {:08X} with value {:0X} (PC = {:016X})\n", paddr, val, (u64)regs.pc);
+  }
+}
+
+template <bool tlb>
+void Mem::Write16(Registers& regs, u32 vaddr, u32 val, s64 pc) {
+  u32 paddr = vaddr;
+  if(!MapVAddr<tlb>(regs, STORE, vaddr, paddr)) {
+    HandleTLBException(regs, vaddr);
+    FireException(regs, GetTLBExceptionCode(regs.cop0.tlbError, STORE), 0, pc);
+  }
+
+  switch(paddr) {
+    case 0x00000000 ... 0x007FFFFF:
+      util::WriteAccess<u16>(mmio.rdp.dram.data(), HALF_ADDRESS(paddr) & RDRAM_DSIZE, val);
+      break;
+    case 0x04000000 ... 0x0403FFFF:
+      val = val << (16 * !(paddr & 2));
+      paddr &= ~3;
+      if(paddr & 0x1000)
+        util::WriteAccess<u32>(mmio.rsp.imem, paddr & IMEM_DSIZE, val);
+      else
+        util::WriteAccess<u32>(mmio.rsp.dmem, paddr & DMEM_DSIZE, val);
+      break;
+    case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
+    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF: mmio.Write(*this, regs, paddr, val); break;
+    case 0x10000000 ... 0x13FFFFFF: break;
+    case 0x1FC007C0 ... 0x1FC007FF:
+      val = val << (16 * !(paddr & 2));
+      paddr &= ~3;
+      util::WriteAccess<u32>(pifRam, paddr & PIF_RAM_DSIZE, htobe32(val));
+      ProcessPIFCommands(pifRam, mmio.si.controller, *this);
+      break;
+    case 0x00800000 ... 0x03FFFFFF: case 0x04200000 ... 0x042FFFFF:
+    case 0x08000000 ... 0x0FFFFFFF: case 0x04900000 ... 0x07FFFFFF:
+    case 0x1FC00800 ... 0x7FFFFFFF: case 0x80000000 ... 0xFFFFFFFF: break;
+    default: util::panic("Unimplemented 16-bit write at address {:08X} with value {:0X} (PC = {:016X})\n", paddr, val, (u64)regs.pc);
+  }
+}
+
+template <bool tlb>
+void Mem::Write32(Registers& regs, u32 vaddr, u32 val, s64 pc) {
+  u32 paddr = vaddr;
+  if(!MapVAddr<tlb>(regs, STORE, vaddr, paddr)) {
+    HandleTLBException(regs, vaddr);
+    FireException(regs, GetTLBExceptionCode(regs.cop0.tlbError, STORE), 0, pc);
+  }
+
+  switch(paddr) {
+    case 0x00000000 ... 0x007FFFFF:
+      util::WriteAccess<u32>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE, val);
+      break;
+    case 0x04000000 ... 0x0403FFFF:
+      if(paddr & 0x1000)
+        util::WriteAccess<u32>(mmio.rsp.imem, paddr & IMEM_DSIZE, val);
+      else
+        util::WriteAccess<u32>(mmio.rsp.dmem, paddr & DMEM_DSIZE, val);
       break;
     case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
     case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF: mmio.Write(*this, regs, paddr, val); break;
@@ -182,39 +302,58 @@ void Mem::Write(Registers& regs, u32 vaddr, T val, s64 pc) {
       }
     } break;
     case 0x13FF0020 ... 0x13FFFFFF:
-      util::WriteAccess<T>(isviewer, paddr & ISVIEWER_DSIZE, be32toh(val));
+      util::WriteAccess<u32>(isviewer, paddr & ISVIEWER_DSIZE, htobe32(val));
       break;
     case 0x1FC007C0 ... 0x1FC007FF:
-      if constexpr (sizeof(T) == 1) {
-        util::WriteAccess<T>(pifRam, BYTE_ADDRESS(paddr) & PIF_RAM_DSIZE, val);
-      } else if constexpr (sizeof(T) == 2) {
-        util::WriteAccess<T>(pifRam, HALF_ADDRESS(paddr) & PIF_RAM_DSIZE, val);
-      } else {
-        util::WriteAccess<T>(pifRam, paddr & PIF_RAM_DSIZE, val);
-      }
+      util::WriteAccess<u32>(pifRam, paddr & PIF_RAM_DSIZE, htobe32(val));
       ProcessPIFCommands(pifRam, mmio.si.controller, *this);
       break;
     case 0x00800000 ... 0x03FFFFFF: case 0x04200000 ... 0x042FFFFF:
     case 0x08000000 ... 0x0FFFFFFF: case 0x04900000 ... 0x07FFFFFF:
     case 0x1FC00800 ... 0x7FFFFFFF: case 0x80000000 ... 0xFFFFFFFF: break;
-    default: util::panic("Unimplemented {}-bit write at address {:08X} with value {:0X} (PC = {:016X})\n", sizeof(T) * 8, paddr, val, (u64)regs.pc);
+    default: util::panic("Unimplemented 32-bit write at address {:08X} with value {:0X} (PC = {:016X})\n", paddr, val, (u64)regs.pc);
   }
 }
 
-template void Mem::Write<u8>(Registers& regs, u32 vaddr, u8 val, s64 pc);
-template void Mem::Write<u16>(Registers& regs, u32 vaddr, u16 val, s64 pc);
-template void Mem::Write<u32>(Registers& regs, u32 vaddr, u32 val, s64 pc);
-template void Mem::Write<u64>(Registers& regs, u32 vaddr, u64 val, s64 pc);
-template void Mem::Write<u8, false>(Registers& regs, u32 vaddr, u8 val, s64 pc);
-template void Mem::Write<u16, false>(Registers& regs, u32 vaddr, u16 val, s64 pc);
-template void Mem::Write<u32, false>(Registers& regs, u32 vaddr, u32 val, s64 pc);
-template void Mem::Write<u64, false>(Registers& regs, u32 vaddr, u64 val, s64 pc);
-template void Mem::Write<s8>(Registers& regs, u32 vaddr, s8 val, s64 pc);
-template void Mem::Write<s16>(Registers& regs, u32 vaddr, s16 val, s64 pc);
-template void Mem::Write<s32>(Registers& regs, u32 vaddr, s32 val, s64 pc);
-template void Mem::Write<s64>(Registers& regs, u32 vaddr, s64 val, s64 pc);
-template void Mem::Write<s8, false>(Registers& regs, u32 vaddr, s8 val, s64 pc);
-template void Mem::Write<s16, false>(Registers& regs, u32 vaddr, s16 val, s64 pc);
-template void Mem::Write<s32, false>(Registers& regs, u32 vaddr, s32 val, s64 pc);
-template void Mem::Write<s64, false>(Registers& regs, u32 vaddr, s64 val, s64 pc);
+template <bool tlb>
+void Mem::Write64(Registers& regs, u32 vaddr, u64 val, s64 pc) {
+  u32 paddr = vaddr;
+  if(!MapVAddr<tlb>(regs, STORE, vaddr, paddr)) {
+    HandleTLBException(regs, vaddr);
+    FireException(regs, GetTLBExceptionCode(regs.cop0.tlbError, STORE), 0, pc);
+  }
+
+  switch(paddr) {
+    case 0x00000000 ... 0x007FFFFF:
+      util::WriteAccess<u64>(mmio.rdp.dram.data(), paddr & RDRAM_DSIZE, val);
+      break;
+    case 0x04000000 ... 0x0403FFFF:
+      val >>= 32;
+      if(paddr & 0x1000)
+        util::WriteAccess<u32>(mmio.rsp.imem, paddr & IMEM_DSIZE, val);
+      else
+        util::WriteAccess<u32>(mmio.rsp.dmem, paddr & DMEM_DSIZE, val);
+      break;
+    case 0x04040000 ... 0x040FFFFF: case 0x04100000 ... 0x041FFFFF:
+    case 0x04300000 ...	0x044FFFFF: case 0x04500000 ... 0x048FFFFF: mmio.Write(*this, regs, paddr, val); break;
+    case 0x10000000 ... 0x13FFFFFF: break;
+    case 0x1FC007C0 ... 0x1FC007FF:
+      util::WriteAccess<u64>(pifRam, paddr & PIF_RAM_DSIZE, htobe64(val));
+      ProcessPIFCommands(pifRam, mmio.si.controller, *this);
+      break;
+    case 0x00800000 ... 0x03FFFFFF: case 0x04200000 ... 0x042FFFFF:
+    case 0x08000000 ... 0x0FFFFFFF: case 0x04900000 ... 0x07FFFFFF:
+    case 0x1FC00800 ... 0x7FFFFFFF: case 0x80000000 ... 0xFFFFFFFF: break;
+    default: util::panic("Unimplemented 64-bit write at address {:08X} with value {:0X} (PC = {:016X})\n", paddr, val, (u64)regs.pc);
+  }
+}
+
+template void Mem::Write8<false>(Registers& regs, u32 vaddr, u32 val, s64 pc);
+template void Mem::Write8<true>(Registers& regs, u32 vaddr, u32 val, s64 pc);
+template void Mem::Write16<false>(Registers& regs, u32 vaddr, u32 val, s64 pc);
+template void Mem::Write16<true>(Registers& regs, u32 vaddr, u32 val, s64 pc);
+template void Mem::Write32<false>(Registers& regs, u32 vaddr, u32 val, s64 pc);
+template void Mem::Write32<true>(Registers& regs, u32 vaddr, u32 val, s64 pc);
+template void Mem::Write64<false>(Registers& regs, u32 vaddr, u64 val, s64 pc);
+template void Mem::Write64<true>(Registers& regs, u32 vaddr, u64 val, s64 pc);
 }
\ No newline at end of file
diff --git a/src/n64/core/Mem.hpp b/src/n64/core/Mem.hpp
index aed0fdbd..b56b9879 100644
--- a/src/n64/core/Mem.hpp
+++ b/src/n64/core/Mem.hpp
@@ -15,10 +15,24 @@ struct Mem {
   [[nodiscard]] auto GetRDRAM() -> u8* {
     return mmio.rdp.dram.data();
   }
-  template <class T, bool tlb = true>
-  T Read(Registers&, u32, s64);
-  template <class T, bool tlb = true>
-  void Write(Registers&, u32, T, s64);
+
+  template <bool tlb = true>
+  u8 Read8(Registers&, u32, s64);
+  template <bool tlb = true>
+  u16 Read16(Registers&, u32, s64);
+  template <bool tlb = true>
+  u32 Read32(Registers&, u32, s64);
+  template <bool tlb = true>
+  u64 Read64(Registers&, u32, s64);
+  template <bool tlb = true>
+  void Write8(Registers&, u32, u32, s64);
+  template <bool tlb = true>
+  void Write16(Registers&, u32, u32, s64);
+  template <bool tlb = true>
+  void Write32(Registers&, u32, u32, s64);
+  template <bool tlb = true>
+  void Write64(Registers&, u32, u64, s64);
+
   u8 pifRam[PIF_RAM_SIZE]{};
 private:
   friend struct SI;
diff --git a/src/n64/core/cpu/instructions.cpp b/src/n64/core/cpu/instructions.cpp
index a3c6a33e..7958458a 100644
--- a/src/n64/core/cpu/instructions.cpp
+++ b/src/n64/core/cpu/instructions.cpp
@@ -175,7 +175,7 @@ void Cpu::lui(u32 instr) {
 
 void Cpu::lb(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
-  regs.gpr[RT(instr)] = mem.Read<s8>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = (s8)mem.Read8(regs, address, regs.oldPC);
 }
 
 void Cpu::lh(Mem& mem, u32 instr) {
@@ -185,7 +185,7 @@ void Cpu::lh(Mem& mem, u32 instr) {
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
 
-  regs.gpr[RT(instr)] = mem.Read<s16>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = (s16)mem.Read16(regs, address, regs.oldPC);
 }
 
 void Cpu::lw(Mem& mem, u32 instr) {
@@ -195,7 +195,7 @@ void Cpu::lw(Mem& mem, u32 instr) {
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
 
-  regs.gpr[RT(instr)] = mem.Read<s32>(regs, address, regs.oldPC);
+  regs.gpr[RT(instr)] = (s32)mem.Read32(regs, address, regs.oldPC);
 }
 
 void Cpu::ll(Mem& mem, u32 instr) {
@@ -205,7 +205,7 @@ void Cpu::ll(Mem& mem, u32 instr) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   } else {
-    regs.gpr[RT(instr)] = mem.Read<s32, false>(regs, physical, regs.oldPC);
+    regs.gpr[RT(instr)] = (s32)mem.Read32<false>(regs, physical, regs.oldPC);
   }
 
   regs.cop0.llbit = true;
@@ -216,7 +216,7 @@ void Cpu::lwl(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
   u32 shift = 8 * ((address ^ 0) & 3);
   u32 mask = 0xFFFFFFFF << shift;
-  u32 data = mem.Read<u32>(regs, address & ~3, regs.oldPC);
+  u32 data = mem.Read32(regs, address & ~3, regs.oldPC);
   s64 rt = regs.gpr[RT(instr)];
   s32 result = (s32)((rt & ~mask) | (data << shift));
   regs.gpr[RT(instr)] = result;
@@ -226,7 +226,7 @@ void Cpu::lwr(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
   u32 shift = 8 * ((address ^ 3) & 3);
   u32 mask = 0xFFFFFFFF >> shift;
-  u32 data = mem.Read<u32>(regs, address & ~3, regs.oldPC);
+  u32 data = mem.Read32(regs, address & ~3, regs.oldPC);
   s64 rt = regs.gpr[RT(instr)];
   s32 result = (s32)((rt & ~mask) | (data >> shift));
   regs.gpr[RT(instr)] = result;
@@ -239,7 +239,7 @@ void Cpu::ld(Mem& mem, u32 instr) {
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
 
-  s64 value = mem.Read<s64>(regs, address, regs.oldPC);
+  s64 value = mem.Read64(regs, address, regs.oldPC);
   regs.gpr[RT(instr)] = value;
 }
 
@@ -250,7 +250,7 @@ void Cpu::lld(Mem& mem, u32 instr) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   } else {
-    regs.gpr[RT(instr)] = mem.Read<s64, false>(regs, physical, regs.oldPC);
+    regs.gpr[RT(instr)] = mem.Read64<false>(regs, physical, regs.oldPC);
   }
 
   regs.cop0.llbit = true;
@@ -261,7 +261,7 @@ void Cpu::ldl(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
   s32 shift = 8 * ((address ^ 0) & 7);
   u64 mask = 0xFFFFFFFFFFFFFFFF << shift;
-  u64 data = mem.Read<u64>(regs, address & ~7, regs.oldPC);
+  u64 data = mem.Read64(regs, address & ~7, regs.oldPC);
   s64 rt = regs.gpr[RT(instr)];
   s64 result = (s64)((rt & ~mask) | (data << shift));
   regs.gpr[RT(instr)] = result;
@@ -271,7 +271,7 @@ void Cpu::ldr(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
   s32 shift = 8 * ((address ^ 7) & 7);
   u64 mask = 0xFFFFFFFFFFFFFFFF >> shift;
-  u64 data = mem.Read<u64>(regs, address & ~7, regs.oldPC);
+  u64 data = mem.Read64(regs, address & ~7, regs.oldPC);
   s64 rt = regs.gpr[RT(instr)];
   s64 result = (s64)((rt & ~mask) | (data >> shift));
   regs.gpr[RT(instr)] = result;
@@ -279,7 +279,7 @@ void Cpu::ldr(Mem& mem, u32 instr) {
 
 void Cpu::lbu(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
-  u8 value = mem.Read<u8>(regs, address, regs.oldPC);
+  u8 value = mem.Read8(regs, address, regs.oldPC);
   regs.gpr[RT(instr)] = value;
 }
 
@@ -290,7 +290,7 @@ void Cpu::lhu(Mem& mem, u32 instr) {
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
 
-  u16 value = mem.Read<u16>(regs, address, regs.oldPC);
+  u16 value = mem.Read16(regs, address, regs.oldPC);
   regs.gpr[RT(instr)] = value;
 }
 
@@ -301,13 +301,13 @@ void Cpu::lwu(Mem& mem, u32 instr) {
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
 
-  u32 value = mem.Read<u32>(regs, address, regs.oldPC);
+  u32 value = mem.Read32(regs, address, regs.oldPC);
   regs.gpr[RT(instr)] = value;
 }
 
 void Cpu::sb(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
-  mem.Write<u8>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+  mem.Write8(regs, address, regs.gpr[RT(instr)], regs.oldPC);
 }
 
 void Cpu::sc(Mem& mem, u32 instr) {
@@ -318,7 +318,7 @@ void Cpu::sc(Mem& mem, u32 instr) {
   }
 
   if(regs.cop0.llbit) {
-    mem.Write<u32>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+    mem.Write32(regs, address, regs.gpr[RT(instr)], regs.oldPC);
   }
 
   regs.gpr[RT(instr)] = (u64)regs.cop0.llbit;
@@ -333,7 +333,7 @@ void Cpu::scd(Mem& mem, u32 instr) {
   }
 
   if(regs.cop0.llbit) {
-    mem.Write<u64>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+    mem.Write64(regs, address, regs.gpr[RT(instr)], regs.oldPC);
   }
 
   regs.gpr[RT(instr)] = (s64)((u64)regs.cop0.llbit);
@@ -347,7 +347,7 @@ void Cpu::sh(Mem& mem, u32 instr) {
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }
 
-  mem.Write<u16>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+  mem.Write16(regs, address, regs.gpr[RT(instr)], regs.oldPC);
 }
 
 void Cpu::sw(Mem& mem, u32 instr) {
@@ -357,7 +357,7 @@ void Cpu::sw(Mem& mem, u32 instr) {
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }
 
-  mem.Write<u32>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+  mem.Write32(regs, address, regs.gpr[RT(instr)], regs.oldPC);
 }
 
 void Cpu::sd(Mem& mem, u32 instr) {
@@ -367,43 +367,43 @@ void Cpu::sd(Mem& mem, u32 instr) {
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }
 
-  mem.Write<u64>(regs, address, regs.gpr[RT(instr)], regs.oldPC);
+  mem.Write64(regs, address, regs.gpr[RT(instr)], regs.oldPC);
 }
 
 void Cpu::sdl(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
   s32 shift = 8 * ((address ^ 0) & 7);
   u64 mask = 0xFFFFFFFFFFFFFFFF >> shift;
-  u64 data = mem.Read<u64>(regs, address & ~7, regs.oldPC);
+  u64 data = mem.Read64(regs, address & ~7, regs.oldPC);
   s64 rt = regs.gpr[RT(instr)];
-  mem.Write<u64>(regs, address & ~7, (data & ~mask) | (rt >> shift), regs.oldPC);
+  mem.Write64(regs, address & ~7, (data & ~mask) | (rt >> shift), regs.oldPC);
 }
 
 void Cpu::sdr(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
   s32 shift = 8 * ((address ^ 7) & 7);
   u64 mask = 0xFFFFFFFFFFFFFFFF << shift;
-  u64 data = mem.Read<u64>(regs, address & ~7, regs.oldPC);
+  u64 data = mem.Read64(regs, address & ~7, regs.oldPC);
   s64 rt = regs.gpr[RT(instr)];
-  mem.Write<u64>(regs, address & ~7, (data & ~mask) | (rt << shift), regs.oldPC);
+  mem.Write64(regs, address & ~7, (data & ~mask) | (rt << shift), regs.oldPC);
 }
 
 void Cpu::swl(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
   u32 shift = 8 * ((address ^ 0) & 3);
   u32 mask = 0xFFFFFFFF >> shift;
-  u32 data = mem.Read<u32>(regs, address & ~3, regs.oldPC);
+  u32 data = mem.Read32(regs, address & ~3, regs.oldPC);
   u32 rt = regs.gpr[RT(instr)];
-  mem.Write<u32>(regs, address & ~3, (data & ~mask) | (rt >> shift), regs.oldPC);
+  mem.Write32(regs, address & ~3, (data & ~mask) | (rt >> shift), regs.oldPC);
 }
 
 void Cpu::swr(Mem& mem, u32 instr) {
   u32 address = regs.gpr[RS(instr)] + (s16)instr;
   u32 shift = 8 * ((address ^ 3) & 3);
   u32 mask = 0xFFFFFFFF << shift;
-  u32 data = mem.Read<u32>(regs, address & ~3, regs.oldPC);
+  u32 data = mem.Read32(regs, address & ~3, regs.oldPC);
   u32 rt = regs.gpr[RT(instr)];
-  mem.Write<u32>(regs, address & ~3, (data & ~mask) | (rt << shift), regs.oldPC);
+  mem.Write32(regs, address & ~3, (data & ~mask) | (rt << shift), regs.oldPC);
 }
 
 void Cpu::ori(u32 instr) {
diff --git a/src/n64/core/cpu/registers/cop1instructions.cpp b/src/n64/core/cpu/registers/cop1instructions.cpp
index c605a202..8c16de54 100644
--- a/src/n64/core/cpu/registers/cop1instructions.cpp
+++ b/src/n64/core/cpu/registers/cop1instructions.cpp
@@ -415,18 +415,18 @@ void Cop1::floorwd(Registers& regs, u32 instr) {
 
 void Cop1::lwc1(Registers& regs, Mem& mem, u32 instr) {
   u32 addr = (s64)(s16)instr + regs.gpr[BASE(instr)];
-  u32 data = mem.Read<u32>(regs, addr, regs.oldPC);
+  u32 data = mem.Read32(regs, addr, regs.oldPC);
   SetReg<u32>(regs.cop0, FT(instr), data);
 }
 
 void Cop1::swc1(Registers& regs, Mem& mem, u32 instr) {
   u32 addr = (s64)(s16)instr + regs.gpr[BASE(instr)];
-  mem.Write<u32>(regs, addr, GetReg<u32>(regs.cop0, FT(instr)), regs.oldPC);
+  mem.Write32(regs, addr, GetReg<u32>(regs.cop0, FT(instr)), regs.oldPC);
 }
 
 void Cop1::ldc1(Registers& regs, Mem& mem, u32 instr) {
   u32 addr = (s64)(s16)instr + regs.gpr[BASE(instr)];
-  u64 data = mem.Read<u64>(regs, addr, regs.oldPC);
+  u64 data = mem.Read64(regs, addr, regs.oldPC);
   SetReg<u64>(regs.cop0, FT(instr), data);
 }
 
@@ -456,7 +456,7 @@ void Cop1::truncld(Registers& regs, u32 instr) {
 
 void Cop1::sdc1(Registers& regs, Mem& mem, u32 instr) {
   u32 addr = (s64)(s16)instr + regs.gpr[BASE(instr)];
-  mem.Write<u64>(regs, addr, GetReg<u64>(regs.cop0, FT(instr)), regs.oldPC);
+  mem.Write64(regs, addr, GetReg<u64>(regs.cop0, FT(instr)), regs.oldPC);
 }
 
 void Cop1::mfc1(Registers& regs, u32 instr) {
diff --git a/src/n64/core/mmio/AI.cpp b/src/n64/core/mmio/AI.cpp
index b7d59b1b..865be940 100644
--- a/src/n64/core/mmio/AI.cpp
+++ b/src/n64/core/mmio/AI.cpp
@@ -81,7 +81,7 @@ void AI::Step(Mem& mem, Registers& regs, int cpuCycles, float volumeL, float vol
 
     u32 address_hi = ((dmaAddr[0] >> 13) + dmaAddrCarry) & 0x7ff;
     dmaAddr[0] = (address_hi << 13) | dmaAddr[0] & 0x1fff;
-    u32 data = mem.Read<u32, false>(regs, dmaAddr[0], regs.pc);
+    u32 data = mem.Read32<false>(regs, dmaAddr[0], regs.pc);
 
     s16 left  = (s16)(data >> 16);
     s16 right = (s16)data;