From f7f121f036529aa7be1cf5d44f45e8275a1cefc9 Mon Sep 17 00:00:00 2001
From: Connor McLaughlin <stenzek@gmail.com>
Date: Wed, 15 Sep 2021 12:23:05 +1000
Subject: [PATCH] Bus: Handle unaligned accesses in safe memory routines

---
 src/core/bus.cpp | 41 ++++++++++++++++++++++++++++++++++-------
 1 file changed, 34 insertions(+), 7 deletions(-)

diff --git a/src/core/bus.cpp b/src/core/bus.cpp
index 10fe5aa2f..842ae6994 100644
--- a/src/core/bus.cpp
+++ b/src/core/bus.cpp
@@ -1961,17 +1961,35 @@ bool SafeReadMemoryByte(VirtualMemoryAddress addr, u8* value)
 
 bool SafeReadMemoryHalfWord(VirtualMemoryAddress addr, u16* value)
 {
-  u32 temp = 0;
-  if (!DoSafeMemoryAccess<MemoryAccessType::Read, MemoryAccessSize::HalfWord>(addr, temp))
+  if ((addr & 1) == 0)
+  {
+    u32 temp = 0;
+    if (!DoSafeMemoryAccess<MemoryAccessType::Read, MemoryAccessSize::HalfWord>(addr, temp))
+      return false;
+
+    *value = Truncate16(temp);
+    return true;
+  }
+
+  u8 low, high;
+  if (!SafeReadMemoryByte(addr, &low) || !SafeReadMemoryByte(addr + 1, &high))
     return false;
 
-  *value = Truncate16(temp);
+  *value = (ZeroExtend16(high) << 8) | ZeroExtend16(low);
   return true;
 }
 
 bool SafeReadMemoryWord(VirtualMemoryAddress addr, u32* value)
 {
-  return DoSafeMemoryAccess<MemoryAccessType::Read, MemoryAccessSize::Word>(addr, *value);
+  if ((addr & 3) == 0)
+    return DoSafeMemoryAccess<MemoryAccessType::Read, MemoryAccessSize::Word>(addr, *value);
+
+  u16 low, high;
+  if (!SafeReadMemoryHalfWord(addr, &low) || !SafeReadMemoryHalfWord(addr + 2, &high))
+    return false;
+
+  *value = (ZeroExtend32(high) << 16) | ZeroExtend32(low);
+  return true;
 }
 
 bool SafeWriteMemoryByte(VirtualMemoryAddress addr, u8 value)
@@ -1982,13 +2000,22 @@ bool SafeWriteMemoryByte(VirtualMemoryAddress addr, u8 value)
 
 bool SafeWriteMemoryHalfWord(VirtualMemoryAddress addr, u16 value)
 {
-  u32 temp = ZeroExtend32(value);
-  return DoSafeMemoryAccess<MemoryAccessType::Write, MemoryAccessSize::HalfWord>(addr, temp);
+  if ((addr & 1) == 0)
+  {
+    u32 temp = ZeroExtend32(value);
+    return DoSafeMemoryAccess<MemoryAccessType::Write, MemoryAccessSize::HalfWord>(addr, temp);
+  }
+
+  return SafeWriteMemoryByte(addr, Truncate8(value)) && SafeWriteMemoryByte(addr + 1, Truncate8(value >> 8));
 }
 
 bool SafeWriteMemoryWord(VirtualMemoryAddress addr, u32 value)
 {
-  return DoSafeMemoryAccess<MemoryAccessType::Write, MemoryAccessSize::Word>(addr, value);
+  if ((addr & 3) == 0)
+    return DoSafeMemoryAccess<MemoryAccessType::Write, MemoryAccessSize::Word>(addr, value);
+
+  return SafeWriteMemoryHalfWord(addr, Truncate16(value >> 16)) &&
+         SafeWriteMemoryHalfWord(addr + 2, Truncate16(value >> 16));
 }
 
 void* GetDirectReadMemoryPointer(VirtualMemoryAddress address, MemoryAccessSize size, TickCount* read_ticks)