[PATCH 18/18] drm/sysfb: vesadrm: Add gamma correction

[Thomas Zimmermann <tzimmermann@suse.de>]

Add palette support and export GAMMA properties via sysfs. User-space
compositors can use this interface for programming gamma ramps or night
mode.

Vesadrm supports palette updates via VGA DAC registers or VESA palette
calls. Up to 256 palette entries are available. Userspace always supplies
gamma ramps of 256 entries. If the native color format does not match
this because pixel component have less then 8 bits, vesadrm interpolates
among the palette entries.

The code uses CamelCase style in a few places to match the VESA manuals.

Signed-off-by: Thomas Zimmermann <tzimmermann@suse.de>
---
 drivers/gpu/drm/sysfb/vesadrm.c | 204 ++++++++++++++++++++++++++++++++
 include/linux/screen_info.h     |   7 ++
 2 files changed, 211 insertions(+)

diff --git a/drivers/gpu/drm/sysfb/vesadrm.c b/drivers/gpu/drm/sysfb/vesadrm.c
index 07f59880ce0f..20cf867ad500 100644
--- a/drivers/gpu/drm/sysfb/vesadrm.c
+++ b/drivers/gpu/drm/sysfb/vesadrm.c
@@ -25,6 +25,7 @@
 
 #include <video/edid.h>
 #include <video/pixel_format.h>
+#include <video/vga.h>
 
 #include "drm_sysfb_helper.h"
 
@@ -33,6 +34,8 @@
 #define DRIVER_MAJOR	1
 #define DRIVER_MINOR	0
 
+#define VESADRM_GAMMA_LUT_SIZE 256
+
 static int vesadrm_get_validated_int(struct drm_device *dev, const char *name,
 				     u64 value, u32 max)
 {
@@ -162,6 +165,14 @@ static const struct drm_format_info *vesadrm_get_format_si(struct drm_device *de
 struct vesadrm_device {
 	struct drm_sysfb_device sysfb;
 
+	/* VESA Protected Mode interface */
+	struct {
+		const u8 *PrimaryPalette;
+	} pmi;
+
+	void (*cmap_write)(struct vesadrm_device *vesa, unsigned int index,
+			   u16 red, u16 green, u16 blue);
+
 	/* modesetting */
 	u32 formats[DRM_SYSFB_PLANE_NFORMATS(1)];
 	struct drm_plane primary_plane;
@@ -170,6 +181,151 @@ struct vesadrm_device {
 	struct drm_connector connector;
 };
 
+static struct vesadrm_device *to_vesadrm_device(struct drm_device *dev)
+{
+	return container_of(to_drm_sysfb_device(dev), struct vesadrm_device, sysfb);
+}
+
+/*
+ * Palette
+ */
+
+static void vesadrm_vga_cmap_write(struct vesadrm_device *vesa, unsigned int index,
+				   u16 red, u16 green, u16 blue)
+{
+	u8 i8, r8, g8, b8;
+
+	if (index > 255)
+		return;
+
+	i8 = index;
+	r8 = red >> 8;
+	g8 = green >> 8;
+	b8 = blue >> 8;
+
+	outb_p(i8, VGA_PEL_IW);
+	outb_p(r8, VGA_PEL_D);
+	outb_p(g8, VGA_PEL_D);
+	outb_p(b8, VGA_PEL_D);
+}
+
+#ifdef __i386__
+static void vesadrm_pmi_cmap_write(struct vesadrm_device *vesa, unsigned int index,
+				   u16 red, u16 green, u16 blue)
+{
+	u32 i32 = index;
+	struct {
+		u8 b8;
+		u8 g8;
+		u8 r8;
+		u8 x8;
+	} PaletteEntry = {
+		blue >> 8,
+		green >> 8,
+		red >> 8,
+		0x00,
+	};
+
+	if (index > 255)
+		return;
+
+	__asm__ __volatile__(
+		"call *(%%esi)"
+		: /* no return value */
+		: "a" (0x4f09),
+		  "b" (0),
+		  "c" (1),
+		  "d" (i32),
+		  "D" (&PaletteEntry),
+		  "S" (&vesa->pmi.PrimaryPalette));
+}
+#endif
+
+static void vesadrm_set_gamma_linear(struct vesadrm_device *vesa,
+				     const struct drm_format_info *format)
+{
+	struct drm_device *dev = &vesa->sysfb.dev;
+	size_t i;
+	u16 r16, g16, b16;
+
+	switch (format->format) {
+	case DRM_FORMAT_XRGB1555:
+		for (i = 0; i < 32; ++i) {
+			r16 = i * 8 + i / 4;
+			r16 |= (r16 << 8) | r16;
+			vesa->cmap_write(vesa, i, r16, r16, r16);
+		}
+		break;
+	case DRM_FORMAT_RGB565:
+		for (i = 0; i < 32; ++i) {
+			r16 = i * 8 + i / 4;
+			r16 |= (r16 << 8) | r16;
+			g16 = i * 4 + i / 16;
+			g16 |= (g16 << 8) | g16;
+			b16 = r16;
+			vesa->cmap_write(vesa, i, r16, g16, b16);
+		}
+		for (i = 32; i < 64; ++i) {
+			g16 = i * 4 + i / 16;
+			g16 |= (g16 << 8) | g16;
+			vesa->cmap_write(vesa, i, 0, g16, 0);
+		}
+		break;
+	case DRM_FORMAT_RGB888:
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_BGRX8888:
+		for (i = 0; i < 256; ++i) {
+			r16 = (i << 8) | i;
+			vesa->cmap_write(vesa, i, r16, r16, r16);
+		}
+		break;
+	default:
+		drm_warn_once(dev, "Unsupported format %p4cc for gamma correction\n",
+			      &format->format);
+		break;
+	}
+}
+
+static void vesadrm_set_gamma_lut(struct vesadrm_device *vesa,
+				  const struct drm_format_info *format,
+				  struct drm_color_lut *lut)
+{
+	struct drm_device *dev = &vesa->sysfb.dev;
+	size_t i;
+	u16 r16, g16, b16;
+
+	switch (format->format) {
+	case DRM_FORMAT_XRGB1555:
+		for (i = 0; i < 32; ++i) {
+			r16 = lut[i * 8 + i / 4].red;
+			g16 = lut[i * 8 + i / 4].green;
+			b16 = lut[i * 8 + i / 4].blue;
+			vesa->cmap_write(vesa, i, r16, g16, b16);
+		}
+		break;
+	case DRM_FORMAT_RGB565:
+		for (i = 0; i < 32; ++i) {
+			r16 = lut[i * 8 + i / 4].red;
+			g16 = lut[i * 4 + i / 16].green;
+			b16 = lut[i * 8 + i / 4].blue;
+			vesa->cmap_write(vesa, i, r16, g16, b16);
+		}
+		for (i = 32; i < 64; ++i)
+			vesa->cmap_write(vesa, i, 0, lut[i * 4 + i / 16].green, 0);
+		break;
+	case DRM_FORMAT_RGB888:
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_BGRX8888:
+		for (i = 0; i < 256; ++i)
+			vesa->cmap_write(vesa, i, lut[i].red, lut[i].green, lut[i].blue);
+		break;
+	default:
+		drm_warn_once(dev, "Unsupported format %p4cc for gamma correction\n",
+			      &format->format);
+		break;
+	}
+}
+
 /*
  * Modesetting
  */
@@ -187,8 +343,36 @@ static const struct drm_plane_funcs vesadrm_primary_plane_funcs = {
 	.destroy = drm_plane_cleanup,
 };
 
+static void vesadrm_crtc_helper_atomic_flush(struct drm_crtc *crtc,
+					     struct drm_atomic_state *state)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_sysfb_device *sysfb = to_drm_sysfb_device(dev);
+	struct vesadrm_device *vesa = to_vesadrm_device(dev);
+	struct drm_crtc_state *crtc_state = drm_atomic_get_new_crtc_state(state, crtc);
+	struct drm_sysfb_crtc_state *sysfb_crtc_state = to_drm_sysfb_crtc_state(crtc_state);
+
+	/*
+	 * The gamma LUT has to be reloaded after changing the primary
+	 * plane's color format.
+	 */
+	if (crtc_state->enable && crtc_state->color_mgmt_changed) {
+		if (sysfb_crtc_state->format == sysfb->fb_format) {
+			if (crtc_state->gamma_lut)
+				vesadrm_set_gamma_lut(vesa,
+						      sysfb_crtc_state->format,
+						      crtc_state->gamma_lut->data);
+			else
+				vesadrm_set_gamma_linear(vesa, sysfb_crtc_state->format);
+		} else {
+			vesadrm_set_gamma_linear(vesa, sysfb_crtc_state->format);
+		}
+	}
+}
+
 static const struct drm_crtc_helper_funcs vesadrm_crtc_helper_funcs = {
 	DRM_SYSFB_CRTC_HELPER_FUNCS,
+	.atomic_flush = vesadrm_crtc_helper_atomic_flush,
 };
 
 static const struct drm_crtc_funcs vesadrm_crtc_funcs = {
@@ -282,6 +466,18 @@ static struct vesadrm_device *vesadrm_device_create(struct drm_driver *drv,
 	drm_dbg(dev, "framebuffer format=%p4cc, size=%dx%d, stride=%d bytes\n",
 		&format->format, width, height, stride);
 
+	if (!__screen_info_vbe_mode_nonvga(si)) {
+		vesa->cmap_write = vesadrm_vga_cmap_write;
+#ifdef __i386__
+	} else {
+		phys_addr_t pmi_base = __screen_info_vesapm_info_base(si);
+		const u16 *pmi_addr = phys_to_virt(pmi_base);
+
+		vesa->pmi.PrimaryPalette = (u8 *)pmi_addr + pmi_addr[2];
+		vesa->cmap_write = vesadrm_pmi_cmap_write;
+#endif
+	}
+
 #ifdef CONFIG_X86
 	if (drm_edid_header_is_valid(edid_info.dummy) == 8)
 		sysfb->edid = edid_info.dummy;
@@ -289,6 +485,8 @@ static struct vesadrm_device *vesadrm_device_create(struct drm_driver *drv,
 	sysfb->fb_mode = drm_sysfb_mode(width, height, 0, 0);
 	sysfb->fb_format = format;
 	sysfb->fb_pitch = stride;
+	if (vesa->cmap_write)
+		sysfb->fb_gamma_lut_size = VESADRM_GAMMA_LUT_SIZE;
 
 	/*
 	 * Memory management
@@ -360,6 +558,12 @@ static struct vesadrm_device *vesadrm_device_create(struct drm_driver *drv,
 		return ERR_PTR(ret);
 	drm_crtc_helper_add(crtc, &vesadrm_crtc_helper_funcs);
 
+	if (sysfb->fb_gamma_lut_size) {
+		ret = drm_mode_crtc_set_gamma_size(crtc, sysfb->fb_gamma_lut_size);
+		if (!ret)
+			drm_crtc_enable_color_mgmt(crtc, 0, false, sysfb->fb_gamma_lut_size);
+	}
+
 	/* Encoder */
 
 	encoder = &vesa->encoder;
diff --git a/include/linux/screen_info.h b/include/linux/screen_info.h
index ab3cffbb58b7..923d68e07679 100644
--- a/include/linux/screen_info.h
+++ b/include/linux/screen_info.h
@@ -126,6 +126,13 @@ static inline unsigned int screen_info_video_type(const struct screen_info *si)
 	return VIDEO_TYPE_CGA;
 }
 
+static inline u32 __screen_info_vesapm_info_base(const struct screen_info *si)
+{
+	if (si->vesapm_seg < 0xc000)
+		return 0;
+	return (si->vesapm_seg << 4) + si->vesapm_off;
+}
+
 ssize_t screen_info_resources(const struct screen_info *si, struct resource *r, size_t num);
 
 u32 __screen_info_lfb_bits_per_pixel(const struct screen_info *si);
-- 
2.48.1