From d64238f76649bfa30966d1ced3be6943494ade96 Mon Sep 17 00:00:00 2001
From: Thomas Zimmermann <tzimmermann@suse.de>
Date: Mon, 25 Apr 2022 09:59:39 +0200
Subject: drm/format-helper: Add RGB565-to-XRGB8888 conversion
Git-commit: e08a99d005588f7f1d0647cdbc3368c98471fa6c
Patch-mainline: v5.19-rc1
References: jsc#PED-1166 jsc#PED-1168 jsc#PED-1170 jsc#PED-1218 jsc#PED-1220 jsc#PED-1222 jsc#PED-1223 jsc#PED-1225

Add a format helper that converts RGB565 to XRGB8888. Use this
function in drm_fb_blit_toio(). Fixes simpledrm output for this
combination of formats.

UEFI and/or Grub will usually set 32-bit output in XRGB8888 format.
The issue can be reproduced by enabling simpledrm and requesting a
console framebuffer of different format on the kernel command line;
for example

  nomodeset video=1024x768-16

In this case, conversion helpers will display nothing on the console.
The patch makes this work by implementing the rsp conversion helpers.
It also enables odd userspace configurations, such as running Xorg
with 16-bit color depth on a 32-bit output buffer.

v2:
	* use helpers for struct drm_rect (Javier)
	* improve commit message (Javier)

Signed-off-by: Thomas Zimmermann <tzimmermann@suse.de>
Reviewed-by: Javier Martinez Canillas <javierm@redhat.com>
Link: https://patchwork.freedesktop.org/patch/msgid/20220425075939.30450-4-tzimmermann@suse.de
Acked-by: Patrik Jakobsson <pjakobsson@suse.de>
---
 drivers/gpu/drm/drm_format_helper.c | 46 +++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)

diff --git a/drivers/gpu/drm/drm_format_helper.c b/drivers/gpu/drm/drm_format_helper.c
index c83e0bb2e8c7..34b7ef443ad2 100644
--- a/drivers/gpu/drm/drm_format_helper.c
+++ b/drivers/gpu/drm/drm_format_helper.c
@@ -411,6 +411,49 @@ void drm_fb_xrgb8888_to_rgb888_toio(void __iomem *dst, unsigned int dst_pitch,
 }
 EXPORT_SYMBOL(drm_fb_xrgb8888_to_rgb888_toio);
 
+static void drm_fb_rgb565_to_xrgb8888_line(void *dbuf, const void *sbuf, unsigned int pixels)
+{
+	u32 *dbuf32 = dbuf;
+	const u16 *sbuf16 = sbuf;
+	unsigned int x;
+
+	for (x = 0; x < pixels; x++, ++sbuf16, ++dbuf32) {
+		u32 val32 = ((*sbuf16 & 0xf800) << 8) |
+			    ((*sbuf16 & 0x07e0) << 5) |
+			    ((*sbuf16 & 0x001f) << 3);
+		*dbuf32 = 0xff000000 | val32 |
+			  ((val32 >> 3) & 0x00070007) |
+			  ((val32 >> 2) & 0x00000300);
+	}
+}
+
+static void drm_fb_rgb565_to_xrgb8888_toio(void __iomem *dst, unsigned int dst_pitch,
+					   const void *vaddr, const struct drm_framebuffer *fb,
+					   const struct drm_rect *clip)
+{
+	size_t linepixels = drm_rect_width(clip);
+	size_t dst_len = linepixels * 4;
+	unsigned int y, lines = drm_rect_height(clip);
+	void *dbuf;
+
+	if (!dst_pitch)
+		dst_pitch = dst_len;
+
+	dbuf = kmalloc(dst_len, GFP_KERNEL);
+	if (!dbuf)
+		return;
+
+	vaddr += clip_offset(clip, fb->pitches[0], 2);
+	for (y = 0; y < lines; y++) {
+		drm_fb_rgb565_to_xrgb8888_line(dbuf, vaddr, linepixels);
+		memcpy_toio(dst, dbuf, dst_len);
+		vaddr += fb->pitches[0];
+		dst += dst_pitch;
+	}
+
+	kfree(dbuf);
+}
+
 static void drm_fb_rgb888_to_xrgb8888_line(void *dbuf, const void *sbuf, unsigned int pixels)
 {
 	u32 *dbuf32 = dbuf;
@@ -628,6 +671,9 @@ int drm_fb_blit_toio(void __iomem *dst, unsigned int dst_pitch, uint32_t dst_for
 		if (fb_format == DRM_FORMAT_RGB888) {
 			drm_fb_rgb888_to_xrgb8888_toio(dst, dst_pitch, vmap, fb, clip);
 			return 0;
+		} else if (fb_format == DRM_FORMAT_RGB565) {
+			drm_fb_rgb565_to_xrgb8888_toio(dst, dst_pitch, vmap, fb, clip);
+			return 0;
 		}
 	} else if (dst_format == DRM_FORMAT_XRGB2101010) {
 		if (fb_format == DRM_FORMAT_XRGB8888) {
-- 
2.38.1