[PATCH 3/5] drm/vkms: add a helper for channel to argb_u16 conversion

[Matheus Alves de Almeida]

Other pixel formats have similar conversions to the one implemented in
the RGB565 helper.

Add a channels_to_argb_u16() helper to convert individual channels into
the internal representation, and use it in argb_u16_from_BGR565() and
argb_u16_from_RGB565().

Signed-off-by: Matheus Alves de Almeida <matheus.aalmeida@xxxxxxxxxxxx>
---
 drivers/gpu/drm/vkms/vkms_formats.c | 50 +++++++++++++++++------------
 1 file changed, 29 insertions(+), 21 deletions(-)

diff --git a/drivers/gpu/drm/vkms/vkms_formats.c b/drivers/gpu/drm/vkms/vkms_formats.c
index b44900761..6e4f58207 100644
--- a/drivers/gpu/drm/vkms/vkms_formats.c
+++ b/drivers/gpu/drm/vkms/vkms_formats.c
@@ -229,24 +229,42 @@ static struct pixel_argb_u16 argb_u16_from_le16161616(__le16 a, __le16 r, __le16
 				       le16_to_cpu(b));
 }
 
-static struct pixel_argb_u16 argb_u16_from_RGB565(const __le16 *pixel)
+static u16 channel_to_u16(unsigned int size, u16 in)
+{
+	s64 fp_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp((1 << size) - 1));
+	s64 fp_channel = drm_int2fixp(in);
+
+	return drm_fixp2int_round(drm_fixp_mul(fp_channel, fp_ratio));
+}
+
+static struct pixel_argb_u16 channels_to_argb_u16(u16 in_r, u16 in_g, u16 in_b, u16 in_a,
+						  unsigned int size_r, unsigned int size_g,
+						  unsigned int size_b, unsigned int size_a)
 {
 	struct pixel_argb_u16 out_pixel;
 
-	s64 fp_rb_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(31));
-	s64 fp_g_ratio = drm_fixp_div(drm_int2fixp(65535), drm_int2fixp(63));
+	out_pixel.r = size_r ? channel_to_u16(size_r, in_r) : in_r;
+	out_pixel.g = size_g ? channel_to_u16(size_g, in_g) : in_g;
+	out_pixel.b = size_b ? channel_to_u16(size_b, in_b) : in_b;
+	out_pixel.a = size_a ? channel_to_u16(size_a, in_a) : in_a;
 
+	return out_pixel;
+}
+
+static struct pixel_argb_u16 argb_u16_from_RGB565(const __le16 *pixel)
+{
 	u16 rgb_565 = le16_to_cpu(*pixel);
-	s64 fp_r = drm_int2fixp((rgb_565 >> 11) & 0x1f);
-	s64 fp_g = drm_int2fixp((rgb_565 >> 5) & 0x3f);
-	s64 fp_b = drm_int2fixp(rgb_565 & 0x1f);
 
-	out_pixel.a = (u16)0xffff;
-	out_pixel.r = drm_fixp2int_round(drm_fixp_mul(fp_r, fp_rb_ratio));
-	out_pixel.g = drm_fixp2int_round(drm_fixp_mul(fp_g, fp_g_ratio));
-	out_pixel.b = drm_fixp2int_round(drm_fixp_mul(fp_b, fp_rb_ratio));
+	return channels_to_argb_u16((rgb_565 >> 11) & 0x1f, (rgb_565 >> 5) & 0x3f,
+				    rgb_565 & 0x1f, 0xffff, 5, 6, 5, 0);
+}
 
-	return out_pixel;
+static struct pixel_argb_u16 argb_u16_from_BGR565(const __le16 *pixel)
+{
+	u16 bgr_565 = le16_to_cpu(*pixel);
+
+	return channels_to_argb_u16(bgr_565 & 0x1f, (bgr_565 >> 5) & 0x3f,
+				    (bgr_565 >> 11) & 0x1f, 0xffff, 5, 6, 5, 0);
 }
 
 static struct pixel_argb_u16 argb_u16_from_gray8(u8 gray)
@@ -259,16 +277,6 @@ static struct pixel_argb_u16 argb_u16_from_grayu16(u16 gray)
 	return argb_u16_from_u16161616(0xFFFF, gray, gray, gray);
 }
 
-static struct pixel_argb_u16 argb_u16_from_BGR565(const __le16 *pixel)
-{
-	struct pixel_argb_u16 out_pixel;
-
-	out_pixel = argb_u16_from_RGB565(pixel);
-	swap(out_pixel.r, out_pixel.b);
-
-	return out_pixel;
-}
-
 VISIBLE_IF_KUNIT
 struct pixel_argb_u16 argb_u16_from_yuv161616(const struct conversion_matrix *matrix,
 					      u16 y, u16 channel_1, u16 channel_2)
-- 
2.43.0