[RFC PATCH v2 3/3] drm: atmel-hlcdc: add clut support for legacy fbdev

[Peter Rosin <peda@axentia.se>]

The clut is not synchronized with the drm gamma_lut state.

Signed-off-by: Peter Rosin <peda@axentia.se>
---
 drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c | 53 ++++++++++++++++++++++++++
 drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c   | 12 +++++-
 drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.h   |  4 ++
 3 files changed, 67 insertions(+), 2 deletions(-)

diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
index 694adcc..4bee26e 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
@@ -140,6 +140,58 @@ static void atmel_hlcdc_crtc_mode_set_nofb(struct drm_crtc *c)
 			   cfg);
 }
 
+static void
+atmel_hlcdc_crtc_load_lut(struct drm_crtc *c)
+{
+	struct atmel_hlcdc_crtc *crtc = drm_crtc_to_atmel_hlcdc_crtc(c);
+	struct atmel_hlcdc_dc *dc = crtc->dc;
+	uint16_t *red = c->gamma_store;
+	uint16_t *green = red + c->gamma_size;
+	uint16_t *blue = green + c->gamma_size;
+	int layer;
+	int idx;
+
+	for (layer = 0; layer < ATMEL_HLCDC_MAX_LAYERS; layer++) {
+		if (!dc->layers[layer])
+			continue;
+
+		for (idx = 0; idx < ATMEL_HLCDC_CLUT_SIZE; idx++) {
+			u32 val = ((red[idx] << 8) & 0xff0000) |
+				(green[idx] & 0xff00) |
+				(blue[idx] >> 8);
+
+			atmel_hlcdc_layer_write_clut(dc->layers[layer],
+						     idx, val);
+		}
+	}
+}
+
+void atmel_hlcdc_gamma_set(struct drm_crtc *c,
+			   u16 r, u16 g, u16 b, int idx)
+{
+	if (idx < 0 || idx >= c->gamma_size)
+		return;
+
+	c->gamma_store[idx] = r;
+	idx += c->gamma_size;
+	c->gamma_store[idx] = g;
+	idx += c->gamma_size;
+	c->gamma_store[idx] = b;
+}
+
+void atmel_hlcdc_gamma_get(struct drm_crtc *c,
+			   u16 *r, u16 *g, u16 *b, int idx)
+{
+	if (idx < 0 || idx >= c->gamma_size)
+		return;
+
+	*r = c->gamma_store[idx];
+	idx += c->gamma_size;
+	*g = c->gamma_store[idx];
+	idx += c->gamma_size;
+	*b = c->gamma_store[idx];
+}
+
 static enum drm_mode_status
 atmel_hlcdc_crtc_mode_valid(struct drm_crtc *c,
 			    const struct drm_display_mode *mode)
@@ -319,6 +371,7 @@ static const struct drm_crtc_helper_funcs lcdc_crtc_helper_funcs = {
 	.mode_set = drm_helper_crtc_mode_set,
 	.mode_set_nofb = atmel_hlcdc_crtc_mode_set_nofb,
 	.mode_set_base = drm_helper_crtc_mode_set_base,
+	.load_lut = atmel_hlcdc_crtc_load_lut,
 	.disable = atmel_hlcdc_crtc_disable,
 	.enable = atmel_hlcdc_crtc_enable,
 	.atomic_check = atmel_hlcdc_crtc_atomic_check,
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
index 4f6ef07..9a09c73 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
@@ -601,6 +601,12 @@ static int atmel_hlcdc_dc_modeset_init(struct drm_device *dev)
 	return 0;
 }
 
+static const struct drm_fb_helper_funcs atmel_hlcdc_fb_cma_helper_funcs = {
+	.gamma_set	= atmel_hlcdc_gamma_set,
+	.gamma_get	= atmel_hlcdc_gamma_get,
+	.fb_probe	= drm_fbdev_cma_create,
+};
+
 static int atmel_hlcdc_dc_load(struct drm_device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev->dev);
@@ -664,8 +670,10 @@ static int atmel_hlcdc_dc_load(struct drm_device *dev)
 
 	platform_set_drvdata(pdev, dev);
 
-	dc->fbdev = drm_fbdev_cma_init(dev, 24,
-			dev->mode_config.num_connector);
+	dc->fbdev = drm_fbdev_cma_init_with_funcs2(dev, 24,
+			dev->mode_config.num_connector,
+			NULL,
+			&atmel_hlcdc_fb_cma_helper_funcs);
 	if (IS_ERR(dc->fbdev))
 		dc->fbdev = NULL;
 
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.h b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.h
index 709f7b9..1b13224 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.h
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.h
@@ -32,6 +32,7 @@
 #include <drm/drm_atomic_helper.h>
 #include <drm/drm_crtc.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_fb_helper.h>
 #include <drm/drm_fb_cma_helper.h>
 #include <drm/drm_gem_cma_helper.h>
 #include <drm/drm_panel.h>
@@ -448,6 +449,9 @@ void atmel_hlcdc_plane_irq(struct atmel_hlcdc_plane *plane);
 int atmel_hlcdc_plane_prepare_disc_area(struct drm_crtc_state *c_state);
 int atmel_hlcdc_plane_prepare_ahb_routing(struct drm_crtc_state *c_state);
 
+void atmel_hlcdc_gamma_set(struct drm_crtc *c, u16 r, u16 g, u16 b, int idx);
+void atmel_hlcdc_gamma_get(struct drm_crtc *c, u16 *r, u16 *g, u16 *b, int idx);
+
 void atmel_hlcdc_crtc_irq(struct drm_crtc *c);
 
 int atmel_hlcdc_crtc_create(struct drm_device *dev);
-- 
2.1.4