1 files changed, 64 insertions, 20 deletions

diff --git a/src/video_core/rasterizer.cpp b/src/video_core/rasterizer.cpp
index 81df09baf..5861c1926 100644
--- a/src/video_core/rasterizer.cpp
+++ b/src/video_core/rasterizer.cpp
@@ -7,12 +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 {
 
@@ -20,59 +22,101 @@ namespace Rasterizer {
 
 static void DrawPixel(int x, int y, const Math::Vec4<u8>& color) {
     const PAddr addr = registers.framebuffer.GetColorBufferPhysicalAddress();
-    u32* color_buffer = reinterpret_cast<u32*>(Memory::GetPointer(PAddrToVAddr(addr)));
 
     // Similarly to textures, the render framebuffer is laid out from bottom to top, too.
     // 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;
+    u8* dst_pixel = Memory::GetPointer(PAddrToVAddr(addr)) + dst_offset;
+
     switch (registers.framebuffer.color_format) {
     case registers.framebuffer.RGBA8:
-    {
-        u32 value = (color.a() << 24) | (color.r() << 16) | (color.g() << 8) | color.b();
-        *(color_buffer + x + y * registers.framebuffer.GetWidth()) = value;
+        Color::EncodeRGBA8(color, dst_pixel);
+        break;
+
+    case registers.framebuffer.RGB8:
+        Color::EncodeRGB8(color, dst_pixel);
+        break;
+
+    case registers.framebuffer.RGB5A1:
+        Color::EncodeRGB5A1(color, dst_pixel);
+        break;
+
+    case registers.framebuffer.RGB565:
+        Color::EncodeRGB565(color, dst_pixel);
+        break;
+
+    case registers.framebuffer.RGBA4:
+        Color::EncodeRGBA4(color, dst_pixel);
         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();
     }
 }
 
 static const Math::Vec4<u8> GetPixel(int x, int y) {
     const PAddr addr = registers.framebuffer.GetColorBufferPhysicalAddress();
-    u32* color_buffer_u32 = reinterpret_cast<u32*>(Memory::GetPointer(PAddrToVAddr(addr)));
 
     y = (registers.framebuffer.height - y);
 
-    u32 value = *(color_buffer_u32 + x + y * registers.framebuffer.GetWidth());
-    Math::Vec4<u8> ret;
-    ret.a() = value >> 24;
-    ret.r() = (value >> 16) & 0xFF;
-    ret.g() = (value >> 8) & 0xFF;
-    ret.b() = value & 0xFF;
-    return ret;
+    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;
+    u8* src_pixel = Memory::GetPointer(PAddrToVAddr(addr)) + src_offset;
+
+    switch (registers.framebuffer.color_format) {
+    case registers.framebuffer.RGBA8:
+        return Color::DecodeRGBA8(src_pixel);
+
+    case registers.framebuffer.RGB8:
+        return Color::DecodeRGB8(src_pixel);
+
+    case registers.framebuffer.RGB5A1:
+        return Color::DecodeRGB5A1(src_pixel);
+
+    case registers.framebuffer.RGB565:
+        return Color::DecodeRGB565(src_pixel);
+
+    case registers.framebuffer.RGBA4:
+        return Color::DecodeRGBA4(src_pixel);
+
+    default:
+        LOG_CRITICAL(Render_Software, "Unknown framebuffer color format %x", registers.framebuffer.color_format.Value());
+        UNIMPLEMENTED();
+    }
+
+    return {};
  }
 
 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