5 files changed, 155 insertions, 85 deletions

diff --git a/src/video_core/CMakeLists.txt b/src/video_core/CMakeLists.txt
index 13c3f7b22..4c1e6449a 100644
--- a/src/video_core/CMakeLists.txt
+++ b/src/video_core/CMakeLists.txt
@@ -19,6 +19,7 @@ set(HEADERS
             renderer_opengl/gl_shaders.h
             renderer_opengl/renderer_opengl.h
             clipper.h
+            color.h
             command_processor.h
             gpu_debugger.h
             math.h
diff --git a/src/video_core/color.h b/src/video_core/color.h
index f095d8ac5..35da901f2 100644
--- a/src/video_core/color.h
+++ b/src/video_core/color.h
@@ -5,47 +5,152 @@
 #pragma once
 
 #include "common/common_types.h"
+#include "video_core/math.h"
 
 namespace Color {
 
 /// Convert a 1-bit color component to 8 bit
-static inline u8 Convert1To8(u8 value) {
+inline u8 Convert1To8(u8 value) {
     return value * 255;
 }
 
 /// Convert a 4-bit color component to 8 bit
-static inline u8 Convert4To8(u8 value) {
+inline u8 Convert4To8(u8 value) {
     return (value << 4) | value;
 }
 
 /// Convert a 5-bit color component to 8 bit
-static inline u8 Convert5To8(u8 value) {
+inline u8 Convert5To8(u8 value) {
     return (value << 3) | (value >> 2);
 }
 
 /// Convert a 6-bit color component to 8 bit
-static inline u8 Convert6To8(u8 value) {
+inline u8 Convert6To8(u8 value) {
     return (value << 2) | (value >> 4);
 }
 
 /// Convert a 8-bit color component to 1 bit
-static inline u8 Convert8To1(u8 value) {
+inline u8 Convert8To1(u8 value) {
     return value >> 7;
 }
 
 /// Convert a 8-bit color component to 4 bit
-static inline u8 Convert8To4(u8 value) {
+inline u8 Convert8To4(u8 value) {
     return value >> 4;
 }
 
 /// Convert a 8-bit color component to 5 bit
-static inline u8 Convert8To5(u8 value) {
+inline u8 Convert8To5(u8 value) {
     return value >> 3;
 }
 
 /// Convert a 8-bit color component to 6 bit
-static inline u8 Convert8To6(u8 value) {
+inline u8 Convert8To6(u8 value) {
     return value >> 2;
 }
 
+/**
+ * Decode a color stored in RGBA8 format
+ * @param bytes Pointer to encoded source color
+ * @return Result color decoded as Math::Vec4<u8>
+ */
+inline const Math::Vec4<u8> DecodeRGBA8(const u8* bytes) {
+    return { bytes[3], bytes[2], bytes[1], bytes[0] };
+}
+
+/**
+ * Decode a color stored in RGB8 format
+ * @param bytes Pointer to encoded source color
+ * @return Result color decoded as Math::Vec4<u8>
+ */
+inline const Math::Vec4<u8> DecodeRGB8(const u8* bytes) {
+    return { bytes[2], bytes[1], bytes[0], 255 };
+}
+
+/**
+ * Decode a color stored in RGB565 format
+ * @param bytes Pointer to encoded source color
+ * @return Result color decoded as Math::Vec4<u8>
+ */
+inline const Math::Vec4<u8> DecodeRGB565(const u8* bytes) {
+    const u16_le pixel = *reinterpret_cast<const u16_le*>(bytes);
+    return { Convert5To8((pixel >> 11) & 0x1F), Convert6To8((pixel >> 5) & 0x3F),
+        Convert5To8(pixel & 0x1F), 255 };
+}
+
+/**
+ * Decode a color stored in RGB5A1 format
+ * @param bytes Pointer to encoded source color
+ * @return Result color decoded as Math::Vec4<u8>
+ */
+inline const Math::Vec4<u8> DecodeRGB5A1(const u8* bytes) {
+    const u16_le pixel = *reinterpret_cast<const u16_le*>(bytes);
+    return { Convert5To8((pixel >> 11) & 0x1F), Convert5To8((pixel >> 6) & 0x1F),
+        Convert5To8((pixel >> 1) & 0x1F), Convert1To8(pixel & 0x1) };
+}
+
+/**
+ * Decode a color stored in RGBA4 format
+ * @param bytes Pointer to encoded source color
+ * @return Result color decoded as Math::Vec4<u8>
+ */
+inline const Math::Vec4<u8> DecodeRGBA4(const u8* bytes) {
+    const u16_le pixel = *reinterpret_cast<const u16_le*>(bytes);
+    return { Convert4To8((pixel >> 12) & 0xF), Convert4To8((pixel >> 8) & 0xF),
+        Convert4To8((pixel >> 4) & 0xF), Convert4To8(pixel & 0xF) };
+}
+
+/**
+ * Encode a color as RGBA8 format
+ * @param color Source color to encode
+ * @param bytes Destination pointer to store encoded color
+ */
+inline void EncodeRGBA8(const Math::Vec4<u8>& color, u8* bytes) {
+    bytes[3] = color.r();
+    bytes[2] = color.g();
+    bytes[1] = color.b();
+    bytes[0] = color.a();
+}
+
+/**
+ * Encode a color as RGB8 format
+ * @param color Source color to encode
+ * @param bytes Destination pointer to store encoded color
+ */
+inline void EncodeRGB8(const Math::Vec4<u8>& color, u8* bytes) {
+    bytes[2] = color.r();
+    bytes[1] = color.g();
+    bytes[0] = color.b();
+}
+
+/**
+ * Encode a color as RGB565 format
+ * @param color Source color to encode
+ * @param bytes Destination pointer to store encoded color
+ */
+inline void EncodeRGB565(const Math::Vec4<u8>& color, u8* bytes) {
+    *reinterpret_cast<u16_le*>(bytes) = (Convert8To5(color.r()) << 11) |
+        (Convert8To6(color.g()) << 5) | Convert8To5(color.b());
+}
+
+/**
+ * Encode a color as RGB5A1 format
+ * @param color Source color to encode
+ * @param bytes Destination pointer to store encoded color
+ */
+inline void EncodeRGB5A1(const Math::Vec4<u8>& color, u8* bytes) {
+    *reinterpret_cast<u16_le*>(bytes) = (Convert8To5(color.r()) << 11) |
+        (Convert8To5(color.g()) << 6) | (Convert8To5(color.b()) << 1) | Convert8To1(color.a());
+}
+
+/**
+ * Encode a color as RGBA4 format
+ * @param color Source color to encode
+ * @param bytes Destination pointer to store encoded color
+ */
+inline void EncodeRGBA4(const Math::Vec4<u8>& color, u8* bytes) {
+    *reinterpret_cast<u16_le*>(bytes) = (Convert8To4(color.r()) << 12) |
+        (Convert8To4(color.g()) << 8) | (Convert8To4(color.b()) << 4) | Convert8To4(color.a());
+}
+
 } // namespace
diff --git a/src/video_core/debug_utils/debug_utils.cpp b/src/video_core/debug_utils/debug_utils.cpp
index 27c246a99..a27d3828c 100644
--- a/src/video_core/debug_utils/debug_utils.cpp
+++ b/src/video_core/debug_utils/debug_utils.cpp
@@ -321,44 +321,32 @@ const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const Texture
     switch (info.format) {
     case Regs::TextureFormat::RGBA8:
     {
-        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] };
+        auto res = Color::DecodeRGBA8(source + VideoCore::GetMortonOffset(x, y, 4));
+        return { res.r(), res.g(), res.b(), disable_alpha ? 255 : res.a() };
     }
 
     case Regs::TextureFormat::RGB8:
     {
-        const u8* source_ptr = source + VideoCore::GetMortonOffset(x, y, 3);
-        return { source_ptr[2], source_ptr[1], source_ptr[0], 255 };
+        auto res = Color::DecodeRGB8(source + VideoCore::GetMortonOffset(x, y, 3));
+        return { res.r(), res.g(), res.b(), 255 };
     }
 
-    case Regs::TextureFormat::RGBA5551:
+    case Regs::TextureFormat::RGB5A1:
     {
-        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;
-        u8 a = source_ptr & 1;
-        return Math::MakeVec<u8>(Color::Convert5To8(r), Color::Convert5To8(g),
-                                 Color::Convert5To8(b), disable_alpha ? 255 : Color::Convert1To8(a));
+        auto res = Color::DecodeRGB5A1(source + VideoCore::GetMortonOffset(x, y, 2));
+        return { res.r(), res.g(), res.b(), disable_alpha ? 255 : res.a() };
     }
 
     case Regs::TextureFormat::RGB565:
     {
-        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);
-        return Math::MakeVec<u8>(r, g, b, 255);
+        auto res = Color::DecodeRGB565(source + VideoCore::GetMortonOffset(x, y, 2));
+        return { res.r(), res.g(), res.b(), 255 };
     }
 
     case Regs::TextureFormat::RGBA4:
     {
-        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);
-        u8 a = Color::Convert4To8(source_ptr[0] & 0xF);
-        return { r, g, b, disable_alpha ? (u8)255 : a };
+        auto res = Color::DecodeRGBA4(source + VideoCore::GetMortonOffset(x, y, 2));
+        return { res.r(), res.g(), res.b(), disable_alpha ? 255 : res.a() };
     }
 
     case Regs::TextureFormat::IA8:
@@ -369,7 +357,7 @@ const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const Texture
             // Show intensity as red, alpha as green
             return { source_ptr[1], source_ptr[0], 0, 255 };
         } else {
-            return { source_ptr[1], source_ptr[1], source_ptr[1], source_ptr[0]};
+            return { source_ptr[1], source_ptr[1], source_ptr[1], source_ptr[0] };
         }
     }
 
diff --git a/src/video_core/pica.h b/src/video_core/pica.h
index 96d0c72fe..b14de9278 100644
--- a/src/video_core/pica.h
+++ b/src/video_core/pica.h
@@ -145,7 +145,7 @@ struct Regs {
     enum class TextureFormat : u32 {
         RGBA8        =  0,
         RGB8         =  1,
-        RGBA5551     =  2,
+        RGB5A1       =  2,
         RGB565       =  3,
         RGBA4        =  4,
         IA8          =  5,
@@ -167,7 +167,7 @@ struct Regs {
         case TextureFormat::RGB8:
             return 6;
 
-        case TextureFormat::RGBA5551:
+        case TextureFormat::RGB5A1:
         case TextureFormat::RGB565:
         case TextureFormat::RGBA4:
         case TextureFormat::IA8:
@@ -413,7 +413,7 @@ struct Regs {
         enum ColorFormat : u32 {
             RGBA8    = 0,
             RGB8     = 1,
-            RGBA5551 = 2,
+            RGB5A1   = 2,
             RGB565   = 3,
             RGBA4    = 4,
         };
diff --git a/src/video_core/rasterizer.cpp b/src/video_core/rasterizer.cpp
index 8c370781a..5861c1926 100644
--- a/src/video_core/rasterizer.cpp
+++ b/src/video_core/rasterizer.cpp
@@ -22,7 +22,6 @@ namespace Rasterizer {
 
 static void DrawPixel(int x, int y, const Math::Vec4<u8>& color) {
     const PAddr addr = registers.framebuffer.GetColorBufferPhysicalAddress();
-    u8* color_buffer = 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.
@@ -31,35 +30,28 @@ static void DrawPixel(int x, int y, const Math::Vec4<u8>& color) {
     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:
-    {
-        u8* pixel = color_buffer + dst_offset;
-        pixel[3] = color.r();
-        pixel[2] = color.g();
-        pixel[1] = color.b();
-        pixel[0] = color.a();
+        Color::EncodeRGBA8(color, dst_pixel);
         break;
-    }
 
-    case registers.framebuffer.RGBA4:
-    {
-        u8* pixel = color_buffer + dst_offset;
-        pixel[1] = (color.r() & 0xF0) | (color.g() >> 4);
-        pixel[0] = (color.b() & 0xF0) | (color.a() >> 4);
+    case registers.framebuffer.RGB8:
+        Color::EncodeRGB8(color, dst_pixel);
         break;
-    }
 
-    case registers.framebuffer.RGBA5551:
-    {
-        u16_le* pixel = (u16_le*)(color_buffer + dst_offset);
-        *pixel = (Color::Convert8To5(color.r()) << 11) |
-                 (Color::Convert8To5(color.g()) << 6)  |
-                 (Color::Convert8To5(color.b()) << 1)  |
-                 Color::Convert8To1(color.a());
+    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.Value());
@@ -69,45 +61,29 @@ static void DrawPixel(int x, int y, const Math::Vec4<u8>& color) {
 
 static const Math::Vec4<u8> GetPixel(int x, int y) {
     const PAddr addr = registers.framebuffer.GetColorBufferPhysicalAddress();
-    u8* color_buffer = Memory::GetPointer(PAddrToVAddr(addr));
 
     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;
-    Math::Vec4<u8> ret;
+    u8* src_pixel = Memory::GetPointer(PAddrToVAddr(addr)) + src_offset;
 
     switch (registers.framebuffer.color_format) {
     case registers.framebuffer.RGBA8:
-    {
-        u8* pixel = color_buffer + src_offset;
-        ret.r() = pixel[3];
-        ret.g() = pixel[2];
-        ret.b() = pixel[1];
-        ret.a() = pixel[0];
-        return ret;
-    }
+        return Color::DecodeRGBA8(src_pixel);
 
-    case registers.framebuffer.RGBA4:
-    {
-        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);
-        ret.a() = Color::Convert4To8(pixel[0] & 0x0F);
-        return ret;
-    }
+    case registers.framebuffer.RGB8:
+        return Color::DecodeRGB8(src_pixel);
 
-    case registers.framebuffer.RGBA5551:
-    {
-        u16_le pixel = *(u16_le*)(color_buffer + src_offset);
-        ret.r() = Color::Convert5To8((pixel >> 11) & 0x1F);
-        ret.g() = Color::Convert5To8((pixel >>  6) & 0x1F);
-        ret.b() = Color::Convert5To8((pixel >>  1) & 0x1F);
-        ret.a() = Color::Convert1To8(pixel & 0x1);
-        return ret;
-    }
+    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());