From e0d873e8159d2b1941b9d9441b561d6e9545b7ba Mon Sep 17 00:00:00 2001
From: Andy Yan <andy.yan@rock-chips.com>
Date: Wed, 13 May 2020 15:45:34 +0800
Subject: [PATCH] drm/rockchip: Convert MCU cmd from rgb565/rgb666 to rgb888

VOP wrongly treated MCU cmd as normal rgb data and pass it
to dither module when output mode is rgb565/rgb666, then
the cmd output from vop io is changed.

Here we convert the MCU cmd data from rgb565/rgb666 to rgb88,
so that we can get the original cmd data after dither module.

Signed-off-by: Andy Yan <andy.yan@rock-chips.com>
Change-Id: I7919dfb9d4f6279b82636d68cd7b211047bf1b46
---
 drivers/gpu/drm/rockchip/rockchip_drm_vop.c | 24 +++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
index e201d14fed4d..9de5523c6e1e 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
@@ -2653,12 +2653,36 @@ static void vop_crtc_close(struct drm_crtc *crtc)
 
 static void vop_crtc_send_mcu_cmd(struct drm_crtc *crtc,  u32 type, u32 value)
 {
+	struct rockchip_crtc_state *s;
 	struct vop *vop = NULL;
 
 	if (!crtc)
 		return;
 
 	vop = to_vop(crtc);
+	s = to_rockchip_crtc_state(crtc->state);
+
+	/*
+	 * VOP wrongly treated MCU cmd as normal rgb data and pass it
+	 * to dither module when output mode is rgb565/rgb666, then
+	 * the cmd output from vop io is changed.
+	 *
+	 * Here we convert the MCU cmd data(8bit) from rgb565/rgb666 to rgb88,
+	 * so that we can get the original cmd data after dither module.
+	 * RGB565->RGB888:
+	 * B = cmd & 0x1f; G = (cmd & 0xe0) >> 5; R = 0
+	 * B + (G << (8 + (8 - 6))) + R
+	 * RGB666->RGB888:
+	 * B = cmd & 0x3f; G = (cmd & 0xc0) >> 6; R = 0
+	 * B + (G << (8 + (8 - 6))) + R
+	 *
+	 * Note: cmd is 8 bit, so R is always zero;
+	 */
+	if (s->output_mode == ROCKCHIP_OUT_MODE_P565)
+		value = ((value & 0x1f) << 3) + ((value & 0xe0) << 5);
+	else if (s->output_mode == ROCKCHIP_OUT_MODE_P666)
+		value = ((value & 0x3f) << 2) + ((value & 0xc0) << 4);
+
 	mutex_lock(&vop->vop_lock);
 	if (vop && vop->is_enabled) {
 		switch (type) {