[PATCH 4/5] drm/vkms: add support for RGB332

[Matheus Alves de Almeida]

Add support for reading and writing the RGB332 pixel format.

Signed-off-by: Matheus Alves de Almeida <matheus.aalmeida@xxxxxxxxxxxx>
---
 drivers/gpu/drm/vkms/vkms_formats.c   | 21 +++++++++++++++++++++
 drivers/gpu/drm/vkms/vkms_plane.c     |  1 +
 drivers/gpu/drm/vkms/vkms_writeback.c |  3 ++-
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/drivers/gpu/drm/vkms/vkms_formats.c b/drivers/gpu/drm/vkms/vkms_formats.c
index 6e4f58207..0c3bb0618 100644
--- a/drivers/gpu/drm/vkms/vkms_formats.c
+++ b/drivers/gpu/drm/vkms/vkms_formats.c
@@ -267,6 +267,12 @@ static struct pixel_argb_u16 argb_u16_from_BGR565(const __le16 *pixel)
 				    (bgr_565 >> 11) & 0x1f, 0xffff, 5, 6, 5, 0);
 }
 
+static struct pixel_argb_u16 argb_u16_from_RGB332(u8 pixel)
+{
+	return channels_to_argb_u16((pixel >> 5) & 0x7, (pixel >> 2) & 0x7,
+				    pixel & 0x3, 0xffff, 3, 3, 2, 0);
+}
+
 static struct pixel_argb_u16 argb_u16_from_gray8(u8 gray)
 {
 	return argb_u16_from_u8888(255, gray, gray, gray);
@@ -475,6 +481,8 @@ READ_LINE_le16161616(XBGR16161616_read_line, px, cpu_to_le16(0xFFFF), px[0], px[
 READ_LINE(RGB565_read_line, px, __le16, argb_u16_from_RGB565, px)
 READ_LINE(BGR565_read_line, px, __le16, argb_u16_from_BGR565, px)
 
+READ_LINE(RGB332_read_line, px, u8, argb_u16_from_RGB332, *px)
+
 READ_LINE(R8_read_line, px, u8, argb_u16_from_gray8, *px)
 
 /*
@@ -690,6 +698,15 @@ static void argb_u16_to_BGR565(u8 *out_pixel, const struct pixel_argb_u16 *in_pi
 	*pixel = cpu_to_le16(b << 11 | g << 5 | r);
 }
 
+static void argb_u16_to_RGB332(u8 *out_pixel, const struct pixel_argb_u16 *in_pixel)
+{
+	u16 r, g, b;
+
+	argb_u16_to_channels(&r, &g, &b, NULL, 3, 3, 2, 0, in_pixel);
+
+	*out_pixel = r << 5 | g << 2 | b;
+}
+
 /**
  * vkms_writeback_row() - Generic loop for all supported writeback format. It is executed just
  * after the blending to write a line in the writeback buffer.
@@ -752,6 +769,8 @@ pixel_read_line_t get_pixel_read_line_function(u32 format)
 		return &RGB565_read_line;
 	case DRM_FORMAT_BGR565:
 		return &BGR565_read_line;
+	case DRM_FORMAT_RGB332:
+		return &RGB332_read_line;
 	case DRM_FORMAT_NV12:
 	case DRM_FORMAT_NV16:
 	case DRM_FORMAT_NV24:
@@ -992,6 +1011,8 @@ pixel_write_t get_pixel_write_function(u32 format)
 		return &argb_u16_to_RGB565;
 	case DRM_FORMAT_BGR565:
 		return &argb_u16_to_BGR565;
+	case DRM_FORMAT_RGB332:
+		return &argb_u16_to_RGB332;
 	default:
 		/*
 		 * This is a bug in vkms_writeback_atomic_check. All the supported
diff --git a/drivers/gpu/drm/vkms/vkms_plane.c b/drivers/gpu/drm/vkms/vkms_plane.c
index 68cb2a333..fff2d4eb5 100644
--- a/drivers/gpu/drm/vkms/vkms_plane.c
+++ b/drivers/gpu/drm/vkms/vkms_plane.c
@@ -29,6 +29,7 @@ static const u32 vkms_formats[] = {
 	DRM_FORMAT_ABGR16161616,
 	DRM_FORMAT_RGB565,
 	DRM_FORMAT_BGR565,
+	DRM_FORMAT_RGB332,
 	DRM_FORMAT_NV12,
 	DRM_FORMAT_NV16,
 	DRM_FORMAT_NV24,
diff --git a/drivers/gpu/drm/vkms/vkms_writeback.c b/drivers/gpu/drm/vkms/vkms_writeback.c
index f64b90908..54330803e 100644
--- a/drivers/gpu/drm/vkms/vkms_writeback.c
+++ b/drivers/gpu/drm/vkms/vkms_writeback.c
@@ -22,7 +22,8 @@ static const u32 vkms_wb_formats[] = {
 	DRM_FORMAT_XRGB16161616,
 	DRM_FORMAT_ARGB16161616,
 	DRM_FORMAT_RGB565,
-	DRM_FORMAT_BGR565
+	DRM_FORMAT_BGR565,
+	DRM_FORMAT_RGB332
 };
 
 static const struct drm_connector_funcs vkms_wb_connector_funcs = {
-- 
2.43.0