Fix LD/LLD/SC/SCD (maybe?)

src/common.hpp

@@ -43,3 +43,8 @@ using m128 = __m128i;
 #define E(x) BASE(x)
 #define ELEMENT_INDEX(i) (7 - (i))
 #define BYTE_INDEX(i) (15 - (i))
+
+
+enum TLBAccessType {
+  LOAD, STORE
+};

src/n64/core/Mem.cpp

@@ -1,6 +1,5 @@
 #include <Mem.hpp>
 #include <fstream>
-#include <util.hpp>
 #include <n64/core/cpu/Registers.hpp>
 #include <n64/core/cpu/registers/Cop0.hpp>
 #include <n64/core/Cpu.hpp>
@@ -41,7 +40,7 @@ void Mem::LoadROM(const std::string& filename) {
 }
 
 template <bool tlb>
-inline bool MapVAddr(Registers& regs, TLBAccessType accessType, u32 vaddr, u32& paddr) {
+bool MapVAddr(Registers& regs, TLBAccessType accessType, u32 vaddr, u32& paddr) {
   paddr = vaddr & 0x1FFFFFFF;
   if constexpr(!tlb) return true;
 
@@ -57,6 +56,9 @@ inline bool MapVAddr(Registers& regs, TLBAccessType accessType, u32 vaddr, u32&
   return false;
 }
 
+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) {
   u32 paddr = vaddr;

src/n64/core/Mem.hpp

@@ -3,6 +3,7 @@
 #include <n64/memory_regions.hpp>
 #include <n64/core/MMIO.hpp>
 #include <vector>
+#include <util.hpp>
 
 namespace n64 {
 struct Registers;
@@ -32,4 +33,7 @@ private:
   u8 isviewer[ISVIEWER_SIZE]{};
   size_t romMask;
 };
+
+template <bool tlb = true>
+bool MapVAddr(Registers& regs, TLBAccessType accessType, u32 vaddr, u32& paddr);
 }

src/n64/core/cpu/instructions.cpp

@@ -200,15 +200,16 @@ void Cpu::lw(Mem& mem, u32 instr) {
 
 void Cpu::ll(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 3) != 0 || (address > 0)) {
+  u32 physical;
+  if (!MapVAddr(regs, LOAD, address, physical)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  } else {
+    regs.gpr[RT(instr)] = mem.Read<s32, false>(regs, physical, regs.oldPC);
   }
 
   regs.cop0.llbit = true;
-  regs.cop0.LLAddr = address;
-
-  regs.gpr[RT(instr)] = mem.Read<s32>(regs, address, regs.oldPC);
+  regs.cop0.LLAddr = physical >> 4;
 }
 
 void Cpu::lwl(Mem& mem, u32 instr) {
@@ -244,16 +245,16 @@ void Cpu::ld(Mem& mem, u32 instr) {
 
 void Cpu::lld(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 7) != 0 || (address > 0)) {
+  u32 physical;
+  if (!MapVAddr(regs, LOAD, address, physical)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
+  } else {
+    regs.gpr[RT(instr)] = mem.Read<s64, false>(regs, physical, regs.oldPC);
   }
 
   regs.cop0.llbit = true;
-  regs.cop0.LLAddr = address;
-
-  s64 value = mem.Read<s64>(regs, address, regs.oldPC);
-  regs.gpr[RT(instr)] = value;
+  regs.cop0.LLAddr = physical >> 4;
 }
 
 void Cpu::ldl(Mem& mem, u32 instr) {

src/n64/core/cpu/registers/Cop0.cpp

@@ -160,7 +160,7 @@ void Cop0::SetReg64(u8 addr, u64 value) {
 
 #define vpn(addr, PageMask) (((((addr) & 0xFFFFFFFFFF) | (((addr) >> 22) & 0x30000000000)) & ~((PageMask) | 0x1FFF)))
 
-TLBEntry* TLBTryMatch(Registers& regs, u32 vaddr, int* match) {
+TLBEntry* TLBTryMatch(Registers& regs, s64 vaddr, int* match) {
   for(int i = 0; i < 32; i++) {
     TLBEntry *entry = &regs.cop0.tlb[i];
     u64 entry_vpn = vpn(entry->entryHi.raw, entry->pageMask.raw);
@@ -180,7 +180,7 @@ TLBEntry* TLBTryMatch(Registers& regs, u32 vaddr, int* match) {
   return nullptr;
 }
 
-bool ProbeTLB(Registers& regs, TLBAccessType access_type, u32 vaddr, u32& paddr, int* match) {
+bool ProbeTLB(Registers& regs, TLBAccessType access_type, s64 vaddr, u32& paddr, int* match) {
   TLBEntry* entry = TLBTryMatch(regs, vaddr, match);
   if(!entry) {
     regs.cop0.tlbError = MISS;

src/n64/core/cpu/registers/Cop0.hpp

@@ -157,10 +157,6 @@ enum TLBError : u8 {
   DISALLOWED_ADDRESS
 };
 
-enum TLBAccessType {
-  LOAD, STORE
-};
-
 union Cop0Context {
   u64 raw;
   struct {
@@ -249,8 +245,8 @@ private:
 struct Registers;
 enum ExceptionCode : u8;
 
-TLBEntry* TLBTryMatch(Registers& regs, u32 vaddr, int* match);
-bool ProbeTLB(Registers& regs, TLBAccessType access_type, u32 vaddr, u32& paddr, int* match);
+TLBEntry* TLBTryMatch(Registers& regs, s64 vaddr, int* match);
+bool ProbeTLB(Registers& regs, TLBAccessType access_type, s64 vaddr, u32& paddr, int* match);
 void HandleTLBException(Registers& regs, u64 vaddr);
 ExceptionCode GetTLBExceptionCode(TLBError error, TLBAccessType access_type);
 }