Fix address error exceptions and TLB match check

src/n64/core/Cpu.cpp

@@ -55,6 +55,8 @@ void FireException(Registers& regs, ExceptionCode code, int cop, s64 pc) {
   regs.cop0.cause.copError = cop;
   regs.cop0.cause.exceptionCode = code;
 
+  s64 exceptionVector = 0;
+
   if(regs.cop0.status.bev) {
     util::panic("BEV bit set!\n");
   } else {
@@ -88,7 +90,11 @@ inline void HandleInterrupt(Registers& regs) {
   }
 }
 
-inline void Cpu::disassembly(u32 instr) const {
+inline void Cpu::disassembly(u32 instr) {
+  auto found = std::find(instructionsLogged.begin(), instructionsLogged.end(), instr);
+
+  if(found == instructionsLogged.end()) {
+    instructionsLogged.push_back(instr);
     size_t count;
     cs_insn *insn;
 
@@ -106,6 +112,7 @@ inline void Cpu::disassembly(u32 instr) const {
       cs_free(insn, count);
     } else
       printf("ERROR: Failed to disassemble given code!\n");
+  }
 }
 
 void Cpu::Step(Mem& mem) {

src/n64/core/Cpu.hpp

@@ -3,6 +3,7 @@
 #include <Mem.hpp>
 #include <util.hpp>
 #include <capstone/capstone.h>
+#include <vector>
 
 namespace n64 {
 struct Cpu {
@@ -21,9 +22,12 @@ struct Cpu {
   Registers regs;
 private:
   csh handle;
-  void disassembly(u32 instr) const;
+
+  void disassembly(u32 instr);
   friend struct Cop1;
 
+  std::vector<u32> instructionsLogged;
+
   void special(Mem&, u32);
   void regimm(u32);
   void Exec(Mem&, u32);

src/n64/core/cpu/instructions.cpp

@@ -2,6 +2,7 @@
 #include <util.hpp>
 
 #define se_imm(x) ((s16)((x) & 0xFFFF))
+#define check_address_error(mask, addr) (((!regs.cop0.is_64bit_addressing) && (s32)(addr) != (addr)) || (((addr) & (mask)) != 0))
 
 namespace n64 {
 
@@ -180,7 +181,7 @@ void Cpu::lb(Mem& mem, u32 instr) {
 
 void Cpu::lh(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if (((address & 1) != 0) || (address > 0)) {
+  if (check_address_error(address, 0b1)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
@@ -190,7 +191,7 @@ void Cpu::lh(Mem& mem, u32 instr) {
 
 void Cpu::lw(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if (((address & 3) != 0) || (address > 0)) {
+  if (check_address_error(address, 0b11)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
@@ -234,7 +235,7 @@ void Cpu::lwr(Mem& mem, u32 instr) {
 
 void Cpu::ld(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 7) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b111)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
@@ -285,7 +286,7 @@ void Cpu::lbu(Mem& mem, u32 instr) {
 
 void Cpu::lhu(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 1) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b1)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
@@ -296,7 +297,7 @@ void Cpu::lhu(Mem& mem, u32 instr) {
 
 void Cpu::lwu(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 3) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b11)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorLoad, 0, regs.oldPC);
   }
@@ -312,7 +313,7 @@ void Cpu::sb(Mem& mem, u32 instr) {
 
 void Cpu::sc(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 3) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b11)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }
@@ -327,7 +328,7 @@ void Cpu::sc(Mem& mem, u32 instr) {
 
 void Cpu::scd(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 7) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b111)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }
@@ -342,7 +343,7 @@ void Cpu::scd(Mem& mem, u32 instr) {
 
 void Cpu::sh(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 1) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b1)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }
@@ -352,7 +353,7 @@ void Cpu::sh(Mem& mem, u32 instr) {
 
 void Cpu::sw(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 3) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b11)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }
@@ -362,7 +363,7 @@ void Cpu::sw(Mem& mem, u32 instr) {
 
 void Cpu::sd(Mem& mem, u32 instr) {
   s64 address = regs.gpr[RS(instr)] + (s16)instr;
-  if ((address & 7) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b11)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }
@@ -423,7 +424,7 @@ void Cpu::nor(u32 instr) {
 void Cpu::j(u32 instr) {
   s32 target = (instr & 0x3ffffff) << 2;
   s64 address = (regs.oldPC & ~0xfffffff) | target;
-  if ((address & 3) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b11)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::DataBusError, 0, regs.oldPC);
   }
@@ -580,7 +581,7 @@ void Cpu::dsra32(u32 instr) {
 
 void Cpu::jr(u32 instr) {
   s64 address = regs.gpr[RS(instr)];
-  if ((address & 3) != 0 || (address > 0)) {
+  if (check_address_error(address, 0b11)) {
     HandleTLBException(regs, address);
     FireException(regs, ExceptionCode::AddressErrorStore, 0, regs.oldPC);
   }

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

@@ -163,13 +163,18 @@ void Cop0::SetReg64(u8 addr, u64 value) {
   }
 }
 
-#define vpn(addr, PageMask) (((((addr) & 0xFFFFFFFFFF) | (((addr) >> 22) & 0x30000000000)) & ~((PageMask) | 0x1FFF)))
+u64 getVPN(u64 addr, u64 pageMask) {
+  u64 mask = pageMask | 0x1fff;
+  u64 vpn = (addr & 0xFFFFFFFFFF) | ((addr >> 22) & 0x30000000000);
+
+  return vpn & ~mask;
+}
 
 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);
+    u64 entry_vpn = getVPN(entry->entryHi.raw, entry->pageMask.raw);
-    u64 vaddr_vpn = vpn(vaddr, entry->pageMask.raw);
+    u64 vaddr_vpn = getVPN(vaddr, entry->pageMask.raw);
 
     bool vpn_match = entry_vpn == vaddr_vpn;
     bool asid_match = entry->global || (regs.cop0.entryHi.asid == entry->entryHi.asid);