From f7785c67e8c26affbf348ef9e257953c970872b8 Mon Sep 17 00:00:00 2001
From: profi200 <fd3194@gmx.de>
Date: Sun, 10 Jan 2021 17:46:38 +0100
Subject: [PATCH] Added 1:1 scaling support.

---
 include/arm11/gpu_cmd_lists.h | 151 +++++++++++++++++++++++++++++++++-
 source/arm11/hardware/lgyfb.c |  18 ++--
 2 files changed, 156 insertions(+), 13 deletions(-)

diff --git a/include/arm11/gpu_cmd_lists.h b/include/arm11/gpu_cmd_lists.h
index c283a91..3e19405 100644
--- a/include/arm11/gpu_cmd_lists.h
+++ b/include/arm11/gpu_cmd_lists.h
@@ -21,11 +21,154 @@
 #include "types.h"
 
 
-#define SCALE_SELECT 1
+#define SCALE_SELECT 2
 
 
 #if SCALE_SELECT == 0
-// 240x160 with bilinear scaling, no filter.
+// 240x160 no scaling, no filter.
+alignas(16) static const u8 gbaGpuInitList[1136] =
+{
+	0x01, 0x00, 0x00, 0x00, 0x10, 0x01, 0x0F, 0x00, 0x00, 0x00, 0x06, 0x03,
+	0x1C, 0x01, 0x2F, 0x80, 0x00, 0x00, 0x03, 0x03, 0xF0, 0xF0, 0x18, 0x01,
+	0xF0, 0xF0, 0x18, 0x01, 0x6E, 0x00, 0x0F, 0x00, 0x03, 0x00, 0x00, 0x00,
+	0x16, 0x01, 0x0F, 0x00, 0x01, 0x00, 0x01, 0x00, 0x17, 0x01, 0x0F, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x1B, 0x01, 0x0F, 0x00, 0x0F, 0x00, 0x00, 0x00,
+	0x12, 0x01, 0x3F, 0x80, 0x0F, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00,
+	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 0x45, 0x00,
+	0x41, 0x00, 0x3F, 0x80, 0x10, 0x11, 0x11, 0x38, 0x00, 0x90, 0x46, 0x00,
+	0x14, 0xAE, 0x47, 0x37, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x68, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x2F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x29, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x53, 0x02, 0x03, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x44, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xCB, 0x02, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x4E, 0xCC, 0x02, 0x7F, 0x00,
+	0x01, 0xF0, 0x07, 0x4E, 0x02, 0x08, 0x02, 0x08, 0x03, 0x18, 0x02, 0x08,
+	0x04, 0x28, 0x02, 0x08, 0x05, 0x38, 0x02, 0x08, 0x06, 0x10, 0x20, 0x4C,
+	0x00, 0x00, 0x00, 0x88, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0xBF, 0x02, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD5, 0x02, 0x0F, 0x00,
+	0x6E, 0x03, 0x00, 0x00, 0xD6, 0x02, 0x6F, 0x00, 0xA1, 0x0A, 0x00, 0x00,
+	0x68, 0xC3, 0x06, 0x00, 0x64, 0xC3, 0x06, 0x00, 0x62, 0xC3, 0x06, 0x00,
+	0x61, 0xC3, 0x06, 0x00, 0x6F, 0x03, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x7F,
+	0xBA, 0x02, 0x0F, 0x00, 0x03, 0x00, 0x00, 0x00, 0xBD, 0x02, 0x0F, 0x00,
+	0x01, 0x00, 0x00, 0x00, 0x4A, 0x02, 0x0F, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0x51, 0x02, 0x0F, 0x00, 0x01, 0x00, 0x00, 0x00, 0x5E, 0x02, 0x01, 0x00,
+	0x02, 0x00, 0x00, 0x00, 0x4F, 0x00, 0x7F, 0x80, 0x00, 0x01, 0x02, 0x03,
+	0x0C, 0x0D, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+	0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+	0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, 0x00, 0x0F, 0x00,
+	0x01, 0x01, 0x00, 0x00, 0x6F, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x29, 0x02, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x52, 0x02, 0x0F, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x54, 0x02, 0x0F, 0x00, 0x00, 0x00, 0x00, 0xA0,
+	0x89, 0x02, 0x0F, 0x00, 0x7B, 0x00, 0x00, 0x00, 0x01, 0x02, 0x1F, 0x80,
+	0x00, 0x00, 0xFC, 0x1F, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0xA0,
+	0xB9, 0x02, 0x0B, 0x00, 0x01, 0x00, 0x00, 0x00, 0x42, 0x02, 0x0F, 0x00,
+	0x10, 0x00, 0x00, 0x00, 0xBB, 0x02, 0x1F, 0x80, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x6D, 0x00, 0x0F, 0x00,
+	0x02, 0x00, 0x00, 0x00, 0x40, 0x00, 0x0F, 0x00, 0x00, 0x00, 0xBF, 0x00,
+	0x4D, 0x00, 0x1F, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x10, 0x00, 0x00, 0x00, 0x04, 0x01, 0x3F, 0x80, 0x10, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x10, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x26, 0x01, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x03, 0x01, 0x0F, 0x00, 0x00, 0x00, 0x76, 0x76, 0x01, 0x01, 0x0F, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x02, 0x01, 0x0F, 0x00, 0x00, 0x01, 0xE4, 0x00,
+	0x00, 0x01, 0x07, 0x00, 0x00, 0x3C, 0x00, 0x80, 0x30, 0x01, 0x0F, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x62, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x18, 0x01, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x61, 0x00, 0x01, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x6A, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x81, 0x00, 0x4F, 0x80, 0x00, 0x01, 0x00, 0x01, 0x02, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x03, 0x01, 0x00, 0x00, 0x00,
+	0x8E, 0x00, 0x0F, 0x00, 0x01, 0x10, 0x01, 0x00, 0x80, 0x00, 0x0B, 0x00,
+	0x00, 0x00, 0x01, 0x00, 0x80, 0x00, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0x8B, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 0x00, 0x07, 0x00,
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFD, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xE1, 0x00, 0x0F, 0x00, 0x03, 0x00, 0x03, 0x00, 0xC0, 0x00, 0x4F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+	0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x0F, 0x00, 0xC8, 0x00, 0x4F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+	0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x0F, 0x00, 0xD0, 0x00, 0x4F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+	0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x0F, 0x00, 0xD8, 0x00, 0x4F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+	0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x0F, 0x00, 0xF0, 0x00, 0x4F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+	0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x0F, 0x00, 0xF8, 0x00, 0x4F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+	0x00, 0x00, 0x00, 0x00, 0x5F, 0x00, 0x00, 0x00, 0xC0, 0x02, 0x3F, 0x80,
+	0xBF, 0x00, 0x00, 0x3E, 0x00, 0x3F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xC0, 0x02, 0x0F, 0x00,
+	0x00, 0x00, 0x80, 0xBF, 0xC1, 0x02, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x88, 0x88, 0x08, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x3F,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0A, 0xD7, 0xA3, 0xBB,
+	0x00, 0x00, 0x80, 0xBF, 0x00, 0x00, 0x80, 0x3F, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x3F, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0xFF, 0x7F, 0xB0, 0x02, 0x0F, 0x00, 0x00, 0x01, 0x00, 0x00,
+	0x5E, 0x02, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x5F, 0x02, 0x0F, 0x00,
+	0x00, 0x00, 0x00, 0x80, 0x27, 0x02, 0x0F, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0x53, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x45, 0x02, 0x01, 0x00,
+	0x0F, 0x00, 0x00, 0x00, 0x32, 0x02, 0x0F, 0x00, 0x3E, 0x00, 0x00, 0x3F,
+	0x33, 0x02, 0x2F, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x45, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x33, 0x02, 0x2F, 0x80, 0x80, 0x3D, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x3E, 0x00, 0x00, 0x3F, 0x33, 0x02, 0x2F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x47, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x33, 0x02, 0x2F, 0x80, 0x80, 0x3D, 0x00, 0x00, 0x00, 0xE0, 0x3E, 0x00,
+	0x3E, 0x00, 0x00, 0x3F, 0x33, 0x02, 0x2F, 0x80, 0x40, 0x46, 0x00, 0x00,
+	0x00, 0x40, 0x45, 0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x02, 0x2F, 0x80,
+	0x00, 0x3F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3E, 0x00, 0x00, 0x3F,
+	0x33, 0x02, 0x2F, 0x80, 0x40, 0x46, 0x00, 0x00, 0x00, 0x40, 0x47, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x33, 0x02, 0x2F, 0x80, 0x00, 0x3F, 0x00, 0x00,
+	0x00, 0xE0, 0x3E, 0x00, 0x01, 0x00, 0x00, 0x00, 0x45, 0x02, 0x01, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x53, 0x02, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0x31, 0x02, 0x0F, 0x00, 0x01, 0x00, 0x00, 0x00, 0x11, 0x01, 0x0F, 0x00,
+	0x01, 0x00, 0x00, 0x00, 0x10, 0x01, 0x0F, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0x63, 0x00, 0x0F, 0x00, 0x78, 0x56, 0x34, 0x12, 0x10, 0x00, 0x0F, 0x00,
+	0x78, 0x56, 0x34, 0x12, 0x10, 0x00, 0x0F, 0x00
+};
+
+// 240x160 no scaling, no filter.
+alignas(16) static const u8 gbaGpuList2[448] =
+{
+	0x01, 0x00, 0x00, 0x00, 0x10, 0x01, 0x0F, 0x00, 0x00, 0x00, 0x06, 0x03,
+	0x1C, 0x01, 0x2F, 0x80, 0x00, 0x00, 0x03, 0x03, 0xF0, 0xF0, 0x18, 0x01,
+	0xF0, 0xF0, 0x18, 0x01, 0x6E, 0x00, 0x0F, 0x00, 0x03, 0x00, 0x00, 0x00,
+	0x16, 0x01, 0x0F, 0x00, 0x01, 0x00, 0x01, 0x00, 0x17, 0x01, 0x0F, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x1B, 0x01, 0x0F, 0x00, 0x0F, 0x00, 0x00, 0x00,
+	0x12, 0x01, 0x3F, 0x80, 0x0F, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00,
+	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 0x45, 0x00,
+	0x41, 0x00, 0x3F, 0x80, 0x10, 0x11, 0x11, 0x38, 0x00, 0x90, 0x46, 0x00,
+	0x14, 0xAE, 0x47, 0x37, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x68, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x2F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
+	0xC0, 0x02, 0x0F, 0x00, 0x00, 0x00, 0x80, 0xBF, 0xC1, 0x02, 0xFF, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x88, 0x88, 0x08, 0x3C, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x80, 0x3F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x0A, 0xD7, 0xA3, 0xBB, 0x00, 0x00, 0x80, 0xBF, 0x00, 0x00, 0x80, 0x3F,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x3F,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x5E, 0x02, 0x02, 0x00,
+	0x01, 0x00, 0x00, 0x00, 0x5F, 0x02, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x80,
+	0x27, 0x02, 0x0F, 0x00, 0x01, 0x00, 0x00, 0x00, 0x53, 0x02, 0x01, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x45, 0x02, 0x01, 0x00, 0x0F, 0x00, 0x00, 0x00,
+	0x32, 0x02, 0x0F, 0x00, 0x3E, 0x00, 0x00, 0x3F, 0x33, 0x02, 0x2F, 0x80,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x45, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x33, 0x02, 0x2F, 0x80, 0x80, 0x3D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x3E, 0x00, 0x00, 0x3F, 0x33, 0x02, 0x2F, 0x80, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x40, 0x47, 0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x02, 0x2F, 0x80,
+	0x80, 0x3D, 0x00, 0x00, 0x00, 0xE0, 0x3E, 0x00, 0x3E, 0x00, 0x00, 0x3F,
+	0x33, 0x02, 0x2F, 0x80, 0x40, 0x46, 0x00, 0x00, 0x00, 0x40, 0x45, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x33, 0x02, 0x2F, 0x80, 0x00, 0x3F, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x3E, 0x00, 0x00, 0x3F, 0x33, 0x02, 0x2F, 0x80,
+	0x40, 0x46, 0x00, 0x00, 0x00, 0x40, 0x47, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x33, 0x02, 0x2F, 0x80, 0x00, 0x3F, 0x00, 0x00, 0x00, 0xE0, 0x3E, 0x00,
+	0x01, 0x00, 0x00, 0x00, 0x45, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x53, 0x02, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x31, 0x02, 0x0F, 0x00,
+	0x01, 0x00, 0x00, 0x00, 0x11, 0x01, 0x0F, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0x10, 0x01, 0x0F, 0x00, 0x01, 0x00, 0x00, 0x00, 0x63, 0x00, 0x0F, 0x00,
+	0x78, 0x56, 0x34, 0x12, 0x10, 0x00, 0x0F, 0x00, 0x78, 0x56, 0x34, 0x12,
+	0x10, 0x00, 0x0F, 0x00
+};
+#elif SCALE_SELECT == 1
+// 240x160 with bilinear scaling to 360x240, no filter.
 alignas(16) static const u8 gbaGpuInitList[1136] =
 {
 	0x01, 0x00, 0x00, 0x00, 0x10, 0x01, 0x0F, 0x00, 0x00, 0x00, 0x06, 0x03,
@@ -125,7 +268,7 @@ alignas(16) static const u8 gbaGpuInitList[1136] =
 	0x78, 0x56, 0x34, 0x12, 0x10, 0x00, 0x0F, 0x00
 };
 
-// 240x160 with bilinear scaling, no filter.
+// 240x160 with bilinear scaling to 360x240, no filter.
 alignas(16) static const u8 gbaGpuList2[448] =
 {
 	0x01, 0x00, 0x00, 0x00, 0x10, 0x01, 0x0F, 0x00, 0x00, 0x00, 0x06, 0x03,
@@ -167,7 +310,7 @@ alignas(16) static const u8 gbaGpuList2[448] =
 	0x78, 0x56, 0x34, 0x12, 0x10, 0x00, 0x0F, 0x00, 0x78, 0x56, 0x34, 0x12,
 	0x10, 0x00, 0x0F, 0x00
 };
-#elif SCALE_SELECT == 1
+#elif SCALE_SELECT == 2
 // 360x240, no filter.
 alignas(16) static const u8 gbaGpuInitList[1136] =
 {
diff --git a/source/arm11/hardware/lgyfb.c b/source/arm11/hardware/lgyfb.c
index 83a4d0a..c4e3ec1 100644
--- a/source/arm11/hardware/lgyfb.c
+++ b/source/arm11/hardware/lgyfb.c
@@ -95,12 +95,10 @@ static void lgyFbDmaIrqHandler(UNUSED u32 intSource)
 	signalEvent(g_frameReadyEvent, false);
 }
 
-static void setScaleMatrixTop(u32 len, u32 patt, const s16 *const matrix)
+static void setScaleMatrix(vu32 *const regs, u8 colorAdjust, u32 len, u32 patt, const s16 *const matrix)
 {
-	REG_LGYFB_TOP_V_LEN = len - 1;
-	REG_LGYFB_TOP_V_PATT = patt;
-	REG_LGYFB_TOP_H_LEN = len - 1;
-	REG_LGYFB_TOP_H_PATT = patt;
+	regs[0] = len - 1; // Reg *_LEN
+	regs[1] = patt;    // Reg *_PATT
 
 	for(u32 y = 0; y < 6; y++)
 	{
@@ -112,8 +110,7 @@ static void setScaleMatrixTop(u32 len, u32 patt, const s16 *const matrix)
 			// For example when converting RGB555 to RGB8 LgyFb lazily shifts the 5 bits up
 			// so 0b00011111 becomes 0b11111000. This creates wrong spacing between colors.
 			// TODO: What is the "+ 8" good for?
-			REG_LGYFB_TOP_V_MATRIX[y][x] = tmp * 0xFF / 0xF8 + 8;
-			REG_LGYFB_TOP_H_MATRIX[y][x] = tmp + 8;
+			regs[16 + (8 * y + x)] = (colorAdjust != 0 ? tmp * 0xFF / colorAdjust + 8 : tmp + 8); // Reg *_MATRIX/*_ARRAY0
 		}
 	}
 }
@@ -180,8 +177,11 @@ void LGYFB_init(KEvent *frameReadyEvent)
 		     0,      0,      0,      0,      0,      0,
 		     0,      0,      0,      0,      0,      0*/
 	};
-	//setScaleMatrixTop(6, 0b00111111, scaleMatrix); // Identity.
-	setScaleMatrixTop(6, 0b00011011, scaleMatrix);
+	// Identity.
+	//setScaleMatrix(&REG_LGYFB_TOP_H_LEN, 0xF8, 6, 0b00111111, scaleMatrix);
+	// Other
+	setScaleMatrix(&REG_LGYFB_TOP_V_LEN, 0xF8, 6, 0b00011011, scaleMatrix);
+	setScaleMatrix(&REG_LGYFB_TOP_H_LEN, 0x00, 6, 0b00011011, scaleMatrix);
 
 	// With RGB8 output solid red and blue are converted to 0xF8 and green to 0xFA.
 	// The green bias exists on the whole range of green colors.