drm/format-helper: Add RGB565-to-XRGB8888 conversion

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

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) {