3 files changed, 91 insertions, 54 deletions

diff --git a/src/video_core/debug_utils/debug_utils.cpp b/src/video_core/debug_utils/debug_utils.cpp
index f436aa541..27c246a99 100644
--- a/src/video_core/debug_utils/debug_utils.cpp
+++ b/src/video_core/debug_utils/debug_utils.cpp
@@ -23,6 +23,7 @@
 #include "video_core/color.h"
 #include "video_core/math.h"
 #include "video_core/pica.h"
+#include "video_core/utils.h"
 
 #include "debug_utils.h"
 
@@ -306,63 +307,33 @@ std::unique_ptr<PicaTrace> FinishPicaTracing()
 }
 
 const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const TextureInfo& info, bool disable_alpha) {
-    // Images are split into 8x8 tiles. Each tile is composed of four 4x4 subtiles each
-    // of which is composed of four 2x2 subtiles each of which is composed of four texels.
-    // Each structure is embedded into the next-bigger one in a diagonal pattern, e.g.
-    // texels are laid out in a 2x2 subtile like this:
-    // 2 3
-    // 0 1
-    //
-    // The full 8x8 tile has the texels arranged like this:
-    //
-    // 42 43 46 47 58 59 62 63
-    // 40 41 44 45 56 57 60 61
-    // 34 35 38 39 50 51 54 55
-    // 32 33 36 37 48 49 52 53
-    // 10 11 14 15 26 27 30 31
-    // 08 09 12 13 24 25 28 29
-    // 02 03 06 07 18 19 22 23
-    // 00 01 04 05 16 17 20 21
-
-    const unsigned int block_width = 8;
-    const unsigned int block_height = 8;
-
     const unsigned int coarse_x = x & ~7;
     const unsigned int coarse_y = y & ~7;
 
-    // Interleave the lower 3 bits of each coordinate to get the intra-block offsets, which are
-    // arranged in a Z-order curve. More details on the bit manipulation at:
-    // https://fgiesen.wordpress.com/2009/12/13/decoding-morton-codes/
-    unsigned int i = (x & 7) | ((y & 7) << 8); // ---- -210
-    i = (i ^ (i << 2)) & 0x1313;               // ---2 --10
-    i = (i ^ (i << 1)) & 0x1515;               // ---2 -1-0
-    i = (i | (i >> 7)) & 0x3F;
-
     if (info.format != Regs::TextureFormat::ETC1 &&
         info.format != Regs::TextureFormat::ETC1A4) {
         // TODO(neobrain): Fix code design to unify vertical block offsets!
         source += coarse_y * info.stride;
     }
-    const unsigned int offset = coarse_x * block_height;
-
+    
     // TODO: Assert that width/height are multiples of block dimensions
 
     switch (info.format) {
     case Regs::TextureFormat::RGBA8:
     {
-        const u8* source_ptr = source + offset * 4 + i * 4;
+        const u8* source_ptr = source + VideoCore::GetMortonOffset(x, y, 4);
         return { source_ptr[3], source_ptr[2], source_ptr[1], disable_alpha ? (u8)255 : source_ptr[0] };
     }
 
     case Regs::TextureFormat::RGB8:
     {
-        const u8* source_ptr = source + offset * 3 + i * 3;
+        const u8* source_ptr = source + VideoCore::GetMortonOffset(x, y, 3);
         return { source_ptr[2], source_ptr[1], source_ptr[0], 255 };
     }
 
     case Regs::TextureFormat::RGBA5551:
     {
-        const u16 source_ptr = *(const u16*)(source + offset * 2 + i * 2);
+        const u16 source_ptr = *(const u16*)(source + VideoCore::GetMortonOffset(x, y, 2));
         u8 r = (source_ptr >> 11) & 0x1F;
         u8 g = ((source_ptr) >> 6) & 0x1F;
         u8 b = (source_ptr >> 1) & 0x1F;
@@ -373,7 +344,7 @@ const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const Texture
 
     case Regs::TextureFormat::RGB565:
     {
-        const u16 source_ptr = *(const u16*)(source + offset * 2 + i * 2);
+        const u16 source_ptr = *(const u16*)(source + VideoCore::GetMortonOffset(x, y, 2));
         u8 r = Color::Convert5To8((source_ptr >> 11) & 0x1F);
         u8 g = Color::Convert6To8(((source_ptr) >> 5) & 0x3F);
         u8 b = Color::Convert5To8((source_ptr) & 0x1F);
@@ -382,7 +353,7 @@ const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const Texture
 
     case Regs::TextureFormat::RGBA4:
     {
-        const u8* source_ptr = source + offset * 2 + i * 2;
+        const u8* source_ptr = source + VideoCore::GetMortonOffset(x, y, 2);
         u8 r = Color::Convert4To8(source_ptr[1] >> 4);
         u8 g = Color::Convert4To8(source_ptr[1] & 0xF);
         u8 b = Color::Convert4To8(source_ptr[0] >> 4);
@@ -392,7 +363,7 @@ const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const Texture
 
     case Regs::TextureFormat::IA8:
     {
-        const u8* source_ptr = source + offset * 2 + i * 2;
+        const u8* source_ptr = source + VideoCore::GetMortonOffset(x, y, 2);
 
         if (disable_alpha) {
             // Show intensity as red, alpha as green
@@ -404,13 +375,13 @@ const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const Texture
 
     case Regs::TextureFormat::I8:
     {
-        const u8* source_ptr = source + offset + i;
+        const u8* source_ptr = source + VideoCore::GetMortonOffset(x, y, 1);
         return { *source_ptr, *source_ptr, *source_ptr, 255 };
     }
 
     case Regs::TextureFormat::A8:
     {
-        const u8* source_ptr = source + offset + i;
+        const u8* source_ptr = source + VideoCore::GetMortonOffset(x, y, 1);
 
         if (disable_alpha) {
             return { *source_ptr, *source_ptr, *source_ptr, 255 };
@@ -421,7 +392,7 @@ const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const Texture
 
     case Regs::TextureFormat::IA4:
     {
-        const u8* source_ptr = source + offset + i;
+        const u8* source_ptr = source + VideoCore::GetMortonOffset(x, y, 1);
 
         u8 i = Color::Convert4To8(((*source_ptr) & 0xF0) >> 4);
         u8 a = Color::Convert4To8((*source_ptr) & 0xF);
@@ -436,9 +407,10 @@ const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const Texture
 
     case Regs::TextureFormat::A4:
     {
-        const u8* source_ptr = source + (offset + i) / 2;
+        u32 morton_offset = VideoCore::GetMortonOffset(x, y, 1);
+        const u8* source_ptr = source + morton_offset / 2;
 
-        u8 a = (i % 2) ? ((*source_ptr & 0xF0) >> 4) : (*source_ptr & 0xF);
+        u8 a = (morton_offset % 2) ? ((*source_ptr & 0xF0) >> 4) : (*source_ptr & 0xF);
         a = Color::Convert4To8(a);
 
         if (disable_alpha) {
diff --git a/src/video_core/rasterizer.cpp b/src/video_core/rasterizer.cpp
index 24dc37856..a7bb0612f 100644
--- a/src/video_core/rasterizer.cpp
+++ b/src/video_core/rasterizer.cpp
@@ -7,13 +7,14 @@
 #include "common/common_types.h"
 #include "common/math_util.h"
 
+#include "core/hw/gpu.h"
+#include "debug_utils/debug_utils.h"
 #include "math.h"
 #include "color.h"
 #include "pica.h"
 #include "rasterizer.h"
 #include "vertex_shader.h"
-
-#include "debug_utils/debug_utils.h"
+#include "video_core/utils.h"
 
 namespace Pica {
 
@@ -27,10 +28,14 @@ static void DrawPixel(int x, int y, const Math::Vec4<u8>& color) {
     // NOTE: The framebuffer height register contains the actual FB height minus one.
     y = (registers.framebuffer.height - y);
 
+    const u32 coarse_y = y & ~7;
+    u32 bytes_per_pixel = GPU::Regs::BytesPerPixel(GPU::Regs::PixelFormat(registers.framebuffer.color_format.Value()));
+    u32 dst_offset = VideoCore::GetMortonOffset(x, y, bytes_per_pixel) + coarse_y * registers.framebuffer.width * bytes_per_pixel;
+
     switch (registers.framebuffer.color_format) {
     case registers.framebuffer.RGBA8:
     {
-        u8* pixel = color_buffer + (x + y * registers.framebuffer.GetWidth()) * 4;
+        u8* pixel = color_buffer + dst_offset;
         pixel[3] = color.r();
         pixel[2] = color.g();
         pixel[1] = color.b();
@@ -40,14 +45,14 @@ static void DrawPixel(int x, int y, const Math::Vec4<u8>& color) {
 
     case registers.framebuffer.RGBA4:
     {
-        u8* pixel = color_buffer + (x + y * registers.framebuffer.GetWidth()) * 2;
+        u8* pixel = color_buffer + dst_offset;
         pixel[1] = (color.r() & 0xF0) | (color.g() >> 4);
         pixel[0] = (color.b() & 0xF0) | (color.a() >> 4);
         break;
     }
 
     default:
-        LOG_CRITICAL(Render_Software, "Unknown framebuffer color format %x", registers.framebuffer.color_format);
+        LOG_CRITICAL(Render_Software, "Unknown framebuffer color format %x", registers.framebuffer.color_format.Value());
         UNIMPLEMENTED();
     }
 }
@@ -58,11 +63,15 @@ static const Math::Vec4<u8> GetPixel(int x, int y) {
 
     y = (registers.framebuffer.height - y);
 
+    const u32 coarse_y = y & ~7;
+    u32 bytes_per_pixel = GPU::Regs::BytesPerPixel(GPU::Regs::PixelFormat(registers.framebuffer.color_format.Value()));
+    u32 src_offset = VideoCore::GetMortonOffset(x, y, bytes_per_pixel) + coarse_y * registers.framebuffer.width * bytes_per_pixel;
+
     switch (registers.framebuffer.color_format) {
     case registers.framebuffer.RGBA8:
     {
         Math::Vec4<u8> ret;
-        u8* pixel = color_buffer + (x + y * registers.framebuffer.GetWidth()) * 4;
+        u8* pixel = color_buffer + src_offset;
         ret.r() = pixel[3];
         ret.g() = pixel[2];
         ret.b() = pixel[1];
@@ -73,7 +82,7 @@ static const Math::Vec4<u8> GetPixel(int x, int y) {
     case registers.framebuffer.RGBA4:
     {
         Math::Vec4<u8> ret;
-        u8* pixel = color_buffer + (x + y * registers.framebuffer.GetWidth()) * 2;
+        u8* pixel = color_buffer + src_offset;
         ret.r() = Color::Convert4To8(pixel[1] >> 4);
         ret.g() = Color::Convert4To8(pixel[1] & 0x0F);
         ret.b() = Color::Convert4To8(pixel[0] >> 4);
@@ -82,7 +91,7 @@ static const Math::Vec4<u8> GetPixel(int x, int y) {
     }
 
     default:
-        LOG_CRITICAL(Render_Software, "Unknown framebuffer color format %x", registers.framebuffer.color_format);
+        LOG_CRITICAL(Render_Software, "Unknown framebuffer color format %x", registers.framebuffer.color_format.Value());
         UNIMPLEMENTED();
     }
 
@@ -91,22 +100,28 @@ static const Math::Vec4<u8> GetPixel(int x, int y) {
 
 static u32 GetDepth(int x, int y) {
     const PAddr addr = registers.framebuffer.GetDepthBufferPhysicalAddress();
-    u16* depth_buffer = reinterpret_cast<u16*>(Memory::GetPointer(PAddrToVAddr(addr)));
+    u8* depth_buffer = Memory::GetPointer(PAddrToVAddr(addr));
 
     y = (registers.framebuffer.height - y);
+    
+    const u32 coarse_y = y & ~7;
+    u32 stride = registers.framebuffer.width * 2;
 
     // Assuming 16-bit depth buffer format until actual format handling is implemented
-    return *(depth_buffer + x + y * registers.framebuffer.GetWidth());
+    return *(u16*)(depth_buffer + VideoCore::GetMortonOffset(x, y, 2) + coarse_y * stride);
 }
 
 static void SetDepth(int x, int y, u16 value) {
     const PAddr addr = registers.framebuffer.GetDepthBufferPhysicalAddress();
-    u16* depth_buffer = reinterpret_cast<u16*>(Memory::GetPointer(PAddrToVAddr(addr)));
+    u8* depth_buffer = Memory::GetPointer(PAddrToVAddr(addr));
 
     y = (registers.framebuffer.height - y);
 
+    const u32 coarse_y = y & ~7;
+    u32 stride = registers.framebuffer.width * 2;
+
     // Assuming 16-bit depth buffer format until actual format handling is implemented
-    *(depth_buffer + x + y * registers.framebuffer.GetWidth()) = value;
+    *(u16*)(depth_buffer + VideoCore::GetMortonOffset(x, y, 2) + coarse_y * stride) = value;
 }
 
 // NOTE: Assuming that rasterizer coordinates are 12.4 fixed-point values
diff --git a/src/video_core/utils.h b/src/video_core/utils.h
index 6fd640425..bda793fa5 100644
--- a/src/video_core/utils.h
+++ b/src/video_core/utils.h
@@ -35,4 +35,54 @@ struct TGAHeader {
  */
 void DumpTGA(std::string filename, short width, short height, u8* raw_data);
 
+/**
+ * Interleave the lower 3 bits of each coordinate to get the intra-block offsets, which are
+ * arranged in a Z-order curve. More details on the bit manipulation at:
+ * https://fgiesen.wordpress.com/2009/12/13/decoding-morton-codes/
+ */
+static inline u32 MortonInterleave(u32 x, u32 y) {
+    u32 i = (x & 7) | ((y & 7) << 8); // ---- -210
+    i = (i ^ (i << 2)) & 0x1313;      // ---2 --10
+    i = (i ^ (i << 1)) & 0x1515;      // ---2 -1-0
+    i = (i | (i >> 7)) & 0x3F;
+    return i;
+}
+
+/**
+ * Calculates the offset of the position of the pixel in Morton order
+ */
+static inline u32 GetMortonOffset(u32 x, u32 y, u32 bytes_per_pixel) {
+    // Images are split into 8x8 tiles. Each tile is composed of four 4x4 subtiles each
+    // of which is composed of four 2x2 subtiles each of which is composed of four texels.
+    // Each structure is embedded into the next-bigger one in a diagonal pattern, e.g.
+    // texels are laid out in a 2x2 subtile like this:
+    // 2 3
+    // 0 1
+    //
+    // The full 8x8 tile has the texels arranged like this:
+    //
+    // 42 43 46 47 58 59 62 63
+    // 40 41 44 45 56 57 60 61
+    // 34 35 38 39 50 51 54 55
+    // 32 33 36 37 48 49 52 53
+    // 10 11 14 15 26 27 30 31
+    // 08 09 12 13 24 25 28 29
+    // 02 03 06 07 18 19 22 23
+    // 00 01 04 05 16 17 20 21
+    //
+    // This pattern is what's called Z-order curve, or Morton order.
+
+    const unsigned int block_width = 8;
+    const unsigned int block_height = 8;
+
+    const unsigned int coarse_x = x & ~7;
+    const unsigned int coarse_y = y & ~7;
+
+    u32 i = VideoCore::MortonInterleave(x, y);
+
+    const unsigned int offset = coarse_x * block_height;
+
+    return (i + offset) * bytes_per_pixel;
+}
+
 } // namespace