[PATCH v2 00/10] drm: Add driver for PowerPC OF displays

@@ -743,6 +743,9 @@ static struct simpledrm_device *simpledrm_device_create(struct drm_driver *drv,
 		drm_err(dev, "no simplefb configuration found\n");
 		return ERR_PTR(-ENODEV);
 	}
+	if (!stride)
+		stride = format->cpp[0] * width;
+
 	sdev->mode = simpledrm_mode(width, height);
 	sdev->format = format;
 	sdev->pitch = stride;

@@ -449,119 +449,6 @@ static int simpledrm_device_init_regulators(struct simpledrm_device *sdev)
 }
 #endif
 
-/*
- *  Simplefb settings
- */
-
-static struct drm_display_mode simpledrm_mode(unsigned int width,
-					      unsigned int height)
-{
-	struct drm_display_mode mode = { SIMPLEDRM_MODE(width, height) };
-
-	mode.clock = mode.hdisplay * mode.vdisplay * 60 / 1000 /* kHz */;
-	drm_mode_set_name(&mode);
-
-	return mode;
-}
-
-static int simpledrm_device_init_fb(struct simpledrm_device *sdev)
-{
-	int width, height, stride;
-	const struct drm_format_info *format;
-	struct drm_device *dev = &sdev->dev;
-	struct platform_device *pdev = sdev->pdev;
-	const struct simplefb_platform_data *pd = dev_get_platdata(&pdev->dev);
-	struct device_node *of_node = pdev->dev.of_node;
-
-	if (pd) {
-		width = simplefb_get_width_pd(dev, pd);
-		if (width < 0)
-			return width;
-		height = simplefb_get_height_pd(dev, pd);
-		if (height < 0)
-			return height;
-		stride = simplefb_get_stride_pd(dev, pd);
-		if (stride < 0)
-			return stride;
-		format = simplefb_get_format_pd(dev, pd);
-		if (IS_ERR(format))
-			return PTR_ERR(format);
-	} else if (of_node) {
-		width = simplefb_get_width_of(dev, of_node);
-		if (width < 0)
-			return width;
-		height = simplefb_get_height_of(dev, of_node);
-		if (height < 0)
-			return height;
-		stride = simplefb_get_stride_of(dev, of_node);
-		if (stride < 0)
-			return stride;
-		format = simplefb_get_format_of(dev, of_node);
-		if (IS_ERR(format))
-			return PTR_ERR(format);
-	} else {
-		drm_err(dev, "no simplefb configuration found\n");
-		return -ENODEV;
-	}
-
-	sdev->mode = simpledrm_mode(width, height);
-	sdev->format = format;
-	sdev->pitch = stride;
-
-	drm_dbg_kms(dev, "display mode={" DRM_MODE_FMT "}\n",
-		    DRM_MODE_ARG(&sdev->mode));
-	drm_dbg_kms(dev,
-		    "framebuffer format=%p4cc, size=%dx%d, stride=%d byte\n",
-		    &format->format, width, height, stride);
-
-	return 0;
-}
-
-/*
- * Memory management
- */
-
-static int simpledrm_device_init_mm(struct simpledrm_device *sdev)
-{
-	struct drm_device *dev = &sdev->dev;
-	struct platform_device *pdev = sdev->pdev;
-	struct resource *res, *mem;
-	void __iomem *screen_base;
-	int ret;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -EINVAL;
-
-	ret = devm_aperture_acquire_from_firmware(dev, res->start, resource_size(res));
-	if (ret) {
-		drm_err(dev, "could not acquire memory range %pr: error %d\n",
-			res, ret);
-		return ret;
-	}
-
-	mem = devm_request_mem_region(&pdev->dev, res->start, resource_size(res),
-				      sdev->dev.driver->name);
-	if (!mem) {
-		/*
-		 * We cannot make this fatal. Sometimes this comes from magic
-		 * spaces our resource handlers simply don't know about. Use
-		 * the I/O-memory resource as-is and try to map that instead.
-		 */
-		drm_warn(dev, "could not acquire memory region %pr\n", res);
-		mem = res;
-	}
-
-	screen_base = devm_ioremap_wc(&pdev->dev, mem->start,
-				      resource_size(mem));
-	if (!screen_base)
-		return -ENOMEM;
-
-	sdev->screen_base = screen_base;
-
-	return 0;
-}
-
 /*
  * Modesetting
  */

@@ -738,6 +625,21 @@ static const struct drm_mode_config_funcs simpledrm_mode_config_funcs = {
 	.atomic_commit = drm_atomic_helper_commit,
 };
 
+/*
+ * Init / Cleanup
+ */
+
+static struct drm_display_mode simpledrm_mode(unsigned int width,
+					      unsigned int height)
+{
+	struct drm_display_mode mode = { SIMPLEDRM_MODE(width, height) };
+
+	mode.clock = mode.hdisplay * mode.vdisplay * 60 / 1000 /* kHz */;
+	drm_mode_set_name(&mode);
+
+	return mode;
+}
+
 static const uint32_t *simpledrm_device_formats(struct simpledrm_device *sdev,
 						size_t *nformats_out)
 {

@@ -777,88 +679,151 @@ static const uint32_t *simpledrm_device_formats(struct simpledrm_device *sdev,
 	return sdev->formats;
 }
 
-static int simpledrm_device_init_modeset(struct simpledrm_device *sdev)
+static struct simpledrm_device *simpledrm_device_create(struct drm_driver *drv,
+							struct platform_device *pdev)
 {
-	struct drm_device *dev = &sdev->dev;
-	struct drm_display_mode *mode = &sdev->mode;
-	struct drm_connector *connector = &sdev->connector;
-	struct drm_simple_display_pipe *pipe = &sdev->pipe;
+	const struct simplefb_platform_data *pd = dev_get_platdata(&pdev->dev);
+	struct device_node *of_node = pdev->dev.of_node;
+	struct simpledrm_device *sdev;
+	struct drm_device *dev;
+	int width, height, stride;
+	const struct drm_format_info *format;
+	struct resource *res, *mem;
+	void __iomem *screen_base;
+	struct drm_connector *connector;
+	struct drm_simple_display_pipe *pipe;
 	unsigned long max_width, max_height;
 	const uint32_t *formats;
 	size_t nformats;
 	int ret;
 
+	sdev = devm_drm_dev_alloc(&pdev->dev, drv, struct simpledrm_device, dev);
+	if (IS_ERR(sdev))
+		return ERR_CAST(sdev);
+	dev = &sdev->dev;
+	sdev->pdev = pdev;
+	platform_set_drvdata(pdev, sdev);
+
+	/*
+	 * Hardware settings
+	 */
+
+	ret = simpledrm_device_init_clocks(sdev);
+	if (ret)
+		return ERR_PTR(ret);
+	ret = simpledrm_device_init_regulators(sdev);
+	if (ret)
+		return ERR_PTR(ret);
+
+	if (pd) {
+		width = simplefb_get_width_pd(dev, pd);
+		if (width < 0)
+			return ERR_PTR(width);
+		height = simplefb_get_height_pd(dev, pd);
+		if (height < 0)
+			return ERR_PTR(height);
+		stride = simplefb_get_stride_pd(dev, pd);
+		if (stride < 0)
+			return ERR_PTR(stride);
+		format = simplefb_get_format_pd(dev, pd);
+		if (IS_ERR(format))
+			return ERR_CAST(format);
+	} else if (of_node) {
+		width = simplefb_get_width_of(dev, of_node);
+		if (width < 0)
+			return ERR_PTR(width);
+		height = simplefb_get_height_of(dev, of_node);
+		if (height < 0)
+			return ERR_PTR(height);
+		stride = simplefb_get_stride_of(dev, of_node);
+		if (stride < 0)
+			return ERR_PTR(stride);
+		format = simplefb_get_format_of(dev, of_node);
+		if (IS_ERR(format))
+			return ERR_CAST(format);
+	} else {
+		drm_err(dev, "no simplefb configuration found\n");
+		return ERR_PTR(-ENODEV);
+	}
+	sdev->mode = simpledrm_mode(width, height);
+	sdev->format = format;
+	sdev->pitch = stride;
+
+	drm_dbg(dev, "display mode={" DRM_MODE_FMT "}\n", DRM_MODE_ARG(&sdev->mode));
+	drm_dbg(dev, "framebuffer format=%p4cc, size=%dx%d, stride=%d byte\n",
+		&format->format, width, height, stride);
+
+	/*
+	 * Memory management
+	 */
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return ERR_PTR(-EINVAL);
+
+	ret = devm_aperture_acquire_from_firmware(dev, res->start, resource_size(res));
+	if (ret) {
+		drm_err(dev, "could not acquire memory range %pr: error %d\n", res, ret);
+		return ERR_PTR(ret);
+	}
+
+	mem = devm_request_mem_region(&pdev->dev, res->start, resource_size(res), drv->name);
+	if (!mem) {
+		/*
+		 * We cannot make this fatal. Sometimes this comes from magic
+		 * spaces our resource handlers simply don't know about. Use
+		 * the I/O-memory resource as-is and try to map that instead.
+		 */
+		drm_warn(dev, "could not acquire memory region %pr\n", res);
+		mem = res;
+	}
+
+	screen_base = devm_ioremap_wc(&pdev->dev, mem->start, resource_size(mem));
+	if (!screen_base)
+		return ERR_PTR(-ENOMEM);
+	sdev->screen_base = screen_base;
+
+	/*
+	 * Modesetting
+	 */
+
 	ret = drmm_mode_config_init(dev);
 	if (ret)
-		return ret;
+		return ERR_PTR(ret);
 
-	max_width = max_t(unsigned long, mode->hdisplay, DRM_SHADOW_PLANE_MAX_WIDTH);
-	max_height = max_t(unsigned long, mode->vdisplay, DRM_SHADOW_PLANE_MAX_HEIGHT);
+	max_width = max_t(unsigned long, width, DRM_SHADOW_PLANE_MAX_WIDTH);
+	max_height = max_t(unsigned long, height, DRM_SHADOW_PLANE_MAX_HEIGHT);
 
-	dev->mode_config.min_width = mode->hdisplay;
+	dev->mode_config.min_width = width;
 	dev->mode_config.max_width = max_width;
-	dev->mode_config.min_height = mode->vdisplay;
+	dev->mode_config.min_height = height;
 	dev->mode_config.max_height = max_height;
-	dev->mode_config.preferred_depth = sdev->format->cpp[0] * 8;
+	dev->mode_config.preferred_depth = format->cpp[0] * 8;
 	dev->mode_config.funcs = &simpledrm_mode_config_funcs;
 
+	connector = &sdev->connector;
 	ret = drm_connector_init(dev, connector, &simpledrm_connector_funcs,
 				 DRM_MODE_CONNECTOR_Unknown);
 	if (ret)
-		return ret;
+		return ERR_PTR(ret);
 	drm_connector_helper_add(connector, &simpledrm_connector_helper_funcs);
 	drm_connector_set_panel_orientation_with_quirk(connector,
 						       DRM_MODE_PANEL_ORIENTATION_UNKNOWN,
-						       mode->hdisplay, mode->vdisplay);
+						       width, height);
 
 	formats = simpledrm_device_formats(sdev, &nformats);
 
+	pipe = &sdev->pipe;
 	ret = drm_simple_display_pipe_init(dev, pipe, &simpledrm_simple_display_pipe_funcs,
 					   formats, nformats, simpledrm_format_modifiers,
 					   connector);
 	if (ret)
-		return ret;
+		return ERR_PTR(ret);
 
 	drm_plane_enable_fb_damage_clips(&pipe->plane);
 
 	drm_mode_config_reset(dev);
 
-	return 0;
-}
-
-/*
- * Init / Cleanup
- */
-
-static struct simpledrm_device *
-simpledrm_device_create(struct drm_driver *drv, struct platform_device *pdev)
-{
-	struct simpledrm_device *sdev;
-	int ret;
-
-	sdev = devm_drm_dev_alloc(&pdev->dev, drv, struct simpledrm_device,
-				  dev);
-	if (IS_ERR(sdev))
-		return ERR_CAST(sdev);
-	sdev->pdev = pdev;
-	platform_set_drvdata(pdev, sdev);
-
-	ret = simpledrm_device_init_clocks(sdev);
-	if (ret)
-		return ERR_PTR(ret);
-	ret = simpledrm_device_init_regulators(sdev);
-	if (ret)
-		return ERR_PTR(ret);
-	ret = simpledrm_device_init_fb(sdev);
-	if (ret)
-		return ERR_PTR(ret);
-	ret = simpledrm_device_init_mm(sdev);
-	if (ret)
-		return ERR_PTR(ret);
-	ret = simpledrm_device_init_modeset(sdev);
-	if (ret)
-		return ERR_PTR(ret);
-
 	return sdev;
 }
 

@@ -13,6 +13,7 @@
 #include <drm/drm_drv.h>
 #include <drm/drm_fb_helper.h>
 #include <drm/drm_format_helper.h>
+#include <drm/drm_framebuffer.h>
 #include <drm/drm_fwfb_helper.h>
 #include <drm/drm_gem_atomic_helper.h>
 #include <drm/drm_gem_framebuffer_helper.h>

@@ -29,6 +30,44 @@
 #define DRIVER_MAJOR	1
 #define DRIVER_MINOR	0
 
+#if !defined(CONFIG_PPC)
+static inline void out_8(void __iomem *addr, int val)
+{ }
+static inline void out_le32(void __iomem *addr, int val)
+{ }
+static inline unsigned int in_le32(const void __iomem *addr)
+{
+	return 0;
+}
+#endif
+
+#define OFDRM_GAMMA_LUT_SIZE	256
+
+/* Definitions used by the Avivo palette  */
+#define AVIVO_DC_LUT_RW_SELECT                  0x6480
+#define AVIVO_DC_LUT_RW_MODE                    0x6484
+#define AVIVO_DC_LUT_RW_INDEX                   0x6488
+#define AVIVO_DC_LUT_SEQ_COLOR                  0x648c
+#define AVIVO_DC_LUT_PWL_DATA                   0x6490
+#define AVIVO_DC_LUT_30_COLOR                   0x6494
+#define AVIVO_DC_LUT_READ_PIPE_SELECT           0x6498
+#define AVIVO_DC_LUT_WRITE_EN_MASK              0x649c
+#define AVIVO_DC_LUT_AUTOFILL                   0x64a0
+#define AVIVO_DC_LUTA_CONTROL                   0x64c0
+#define AVIVO_DC_LUTA_BLACK_OFFSET_BLUE         0x64c4
+#define AVIVO_DC_LUTA_BLACK_OFFSET_GREEN        0x64c8
+#define AVIVO_DC_LUTA_BLACK_OFFSET_RED          0x64cc
+#define AVIVO_DC_LUTA_WHITE_OFFSET_BLUE         0x64d0
+#define AVIVO_DC_LUTA_WHITE_OFFSET_GREEN        0x64d4
+#define AVIVO_DC_LUTA_WHITE_OFFSET_RED          0x64d8
+#define AVIVO_DC_LUTB_CONTROL                   0x6cc0
+#define AVIVO_DC_LUTB_BLACK_OFFSET_BLUE         0x6cc4
+#define AVIVO_DC_LUTB_BLACK_OFFSET_GREEN        0x6cc8
+#define AVIVO_DC_LUTB_BLACK_OFFSET_RED          0x6ccc
+#define AVIVO_DC_LUTB_WHITE_OFFSET_BLUE         0x6cd0
+#define AVIVO_DC_LUTB_WHITE_OFFSET_GREEN        0x6cd4
+#define AVIVO_DC_LUTB_WHITE_OFFSET_RED          0x6cd8
+
 enum ofdrm_model {
 	OFDRM_MODEL_UNKNOWN,
 	OFDRM_MODEL_MACH64, /* ATI Mach64 */

@@ -209,7 +248,14 @@ static enum ofdrm_model display_get_model_of(struct drm_device *dev, struct devi
  * Open Firmware display device
  */
 
+struct ofdrm_device;
+
 struct ofdrm_device_funcs {
+	void __iomem *(*cmap_ioremap)(struct ofdrm_device *odev,
+				      struct device_node *of_node,
+				      u64 fb_bas);
+	void (*cmap_write)(struct ofdrm_device *odev, unsigned char index,
+			   unsigned char r, unsigned char g, unsigned char b);
 };
 
 struct ofdrm_device {

@@ -221,6 +267,9 @@ struct ofdrm_device {
 	/* firmware framebuffer */
 	struct drm_fwfb fwfb;
 
+	/* colormap */
+	void __iomem *cmap_base;
+
 	/* modesetting */
 	uint32_t formats[8];
 	struct drm_plane primary_plane;

@@ -333,12 +382,322 @@ static struct resource *ofdrm_find_fb_resource(struct ofdrm_device *odev,
 	return max_res;
 }
 
+/*
+ * Colormap / Palette
+ */
+
+static void __iomem *get_cmap_address_of(struct ofdrm_device *odev, struct device_node *of_node,
+					 int bar_no, unsigned long offset, unsigned long size)
+{
+	struct drm_device *dev = &odev->dev;
+	const __be32 *addr_p;
+	u64 max_size, address;
+	unsigned int flags;
+	void __iomem *mem;
+
+	addr_p = of_get_pci_address(of_node, bar_no, &max_size, &flags);
+	if (!addr_p)
+		addr_p = of_get_address(of_node, bar_no, &max_size, &flags);
+	if (!addr_p)
+		return ERR_PTR(-ENODEV);
+
+	if ((flags & (IORESOURCE_IO | IORESOURCE_MEM)) == 0)
+		return ERR_PTR(-ENODEV);
+
+	if ((offset + size) >= max_size)
+		return ERR_PTR(-ENODEV);
+
+	address = of_translate_address(of_node, addr_p);
+	if (address == OF_BAD_ADDR)
+		return ERR_PTR(-ENODEV);
+
+	mem = devm_ioremap(dev->dev, address + offset, size);
+	if (!mem)
+		return ERR_PTR(-ENOMEM);
+
+	return mem;
+}
+
+static void __iomem *ofdrm_mach64_cmap_ioremap(struct ofdrm_device *odev,
+					       struct device_node *of_node,
+					       u64 fb_base)
+{
+	struct drm_device *dev = &odev->dev;
+	u64 address;
+	void __iomem *cmap_base;
+
+	address = fb_base & 0xff000000ul;
+	address += 0x7ff000;
+
+	cmap_base = devm_ioremap(dev->dev, address, 0x1000);
+	if (!cmap_base)
+		return ERR_PTR(-ENOMEM);
+
+	return cmap_base;
+}
+
+static void ofdrm_mach64_cmap_write(struct ofdrm_device *odev, unsigned char index,
+				    unsigned char r, unsigned char g, unsigned char b)
+{
+	void __iomem *addr = odev->cmap_base + 0xcc0;
+	void __iomem *data = odev->cmap_base + 0xcc0 + 1;
+
+	writeb(index, addr);
+	writeb(r, data);
+	writeb(g, data);
+	writeb(b, data);
+}
+
+static void __iomem *ofdrm_rage128_cmap_ioremap(struct ofdrm_device *odev,
+						struct device_node *of_node,
+						u64 fb_base)
+{
+	return get_cmap_address_of(odev, of_node, 2, 0, 0x1fff);
+}
+
+static void ofdrm_rage128_cmap_write(struct ofdrm_device *odev, unsigned char index,
+				     unsigned char r, unsigned char g, unsigned char b)
+{
+	void __iomem *addr = odev->cmap_base + 0xb0;
+	void __iomem *data = odev->cmap_base + 0xb4;
+	int color = (r << 16) | (g << 8) | b;
+
+	out_8(addr, index);
+	out_le32(data, color);
+}
+
+static void __iomem *ofdrm_rage_m3a_cmap_ioremap(struct ofdrm_device *odev,
+						 struct device_node *of_node,
+						 u64 fb_base)
+{
+	return get_cmap_address_of(odev, of_node, 2, 0, 0x1fff);
+}
+
+static void ofdrm_rage_m3a_cmap_write(struct ofdrm_device *odev, unsigned char index,
+				      unsigned char r, unsigned char g, unsigned char b)
+{
+	void __iomem *dac_ctl = odev->cmap_base + 0x58;
+	void __iomem *addr = odev->cmap_base + 0xb0;
+	void __iomem *data = odev->cmap_base + 0xb4;
+	int color = (r << 16) | (g << 8) | b;
+	unsigned int val;
+
+	/* Clear PALETTE_ACCESS_CNTL in DAC_CNTL */
+	val = in_le32(dac_ctl);
+	val &= ~0x20;
+	out_le32(dac_ctl, val);
+
+	/* Set color at palette index */
+	out_8(addr, index);
+	out_le32(data, color);
+}
+
+static void __iomem *ofdrm_rage_m3b_cmap_ioremap(struct ofdrm_device *odev,
+						 struct device_node *of_node,
+						 u64 fb_base)
+{
+	return get_cmap_address_of(odev, of_node, 2, 0, 0x1fff);
+}
+
+static void ofdrm_rage_m3b_cmap_write(struct ofdrm_device *odev, unsigned char index,
+				      unsigned char r, unsigned char g, unsigned char b)
+{
+	void __iomem *dac_ctl = odev->cmap_base + 0x58;
+	void __iomem *addr = odev->cmap_base + 0xb0;
+	void __iomem *data = odev->cmap_base + 0xb4;
+	int color = (r << 16) | (g << 8) | b;
+	unsigned int val;
+
+	/* Set PALETTE_ACCESS_CNTL in DAC_CNTL */
+	val = in_le32(dac_ctl);
+	val |= 0x20;
+	out_le32(dac_ctl, val);
+
+	/* Set color at palette index */
+	out_8(addr, index);
+	out_le32(data, color);
+}
+
+static void __iomem *ofdrm_radeon_cmap_ioremap(struct ofdrm_device *odev,
+					       struct device_node *of_node,
+					       u64 fb_base)
+{
+	return get_cmap_address_of(odev, of_node, 1, 0, 0x1fff);
+}
+
+static void __iomem *ofdrm_gxt2000_cmap_ioremap(struct ofdrm_device *odev,
+						struct device_node *of_node,
+						u64 fb_base)
+{
+	return get_cmap_address_of(odev, of_node, 0, 0x6000, 0x1000);
+}
+
+static void ofdrm_gxt2000_cmap_write(struct ofdrm_device *odev, unsigned char index,
+				     unsigned char r, unsigned char g, unsigned char b)
+{
+	void __iomem *data = ((unsigned int __iomem *)odev->cmap_base) + index;
+	int color = (r << 16) | (g << 8) | b;
+
+	out_le32(data, color);
+}
+
+static void __iomem *ofdrm_avivo_cmap_ioremap(struct ofdrm_device *odev,
+					      struct device_node *of_node,
+					      u64 fb_base)
+{
+	struct device_node *of_parent;
+	void __iomem *cmap_base;
+
+	of_parent = of_get_parent(of_node);
+	cmap_base = get_cmap_address_of(odev, of_parent, 0, 0, 0x10000);
+	of_node_put(of_parent);
+
+	return cmap_base;
+}
+
+static void ofdrm_avivo_cmap_write(struct ofdrm_device *odev, unsigned char index,
+				   unsigned char r, unsigned char g, unsigned char b)
+{
+	void __iomem *lutsel = odev->cmap_base + AVIVO_DC_LUT_RW_SELECT;
+	void __iomem *addr = odev->cmap_base + AVIVO_DC_LUT_RW_INDEX;
+	void __iomem *data = odev->cmap_base + AVIVO_DC_LUT_30_COLOR;
+	u32 color = (r << 22) | (g << 12) | (b << 2);
+
+	/* Write to both LUTs for now */
+
+	writel(1, lutsel);
+	writeb(index, addr);
+	writel(color, data);
+
+	writel(0, lutsel);
+	writeb(index, addr);
+	writel(color, data);
+}
+
+static void __iomem *ofdrm_qemu_cmap_ioremap(struct ofdrm_device *odev,
+					     struct device_node *of_node,
+					     u64 fb_base)
+{
+#ifdef __BIG_ENDIAN
+	static const __be32 io_of_addr[3] = { 0x01000000, 0x0, 0x0 };
+#else
+	static const __be32 io_of_addr[3] = { 0x00000001, 0x0, 0x0 };
+#endif
+
+	struct drm_device *dev = &odev->dev;
+	u64 address;
+	void __iomem *cmap_base;
+
+	address = of_translate_address(of_node, io_of_addr);
+	if (address == OF_BAD_ADDR)
+		return ERR_PTR(-ENODEV);
+
+	cmap_base = devm_ioremap(dev->dev, address + 0x3c8, 2);
+	if (!cmap_base)
+		return ERR_PTR(-ENOMEM);
+
+	return cmap_base;
+}
+
+static void ofdrm_qemu_cmap_write(struct ofdrm_device *odev, unsigned char index,
+				  unsigned char r, unsigned char g, unsigned char b)
+{
+	void __iomem *addr = odev->cmap_base;
+	void __iomem *data = odev->cmap_base + 1;
+
+	writeb(index, addr);
+	writeb(r, data);
+	writeb(g, data);
+	writeb(b, data);
+}
+
+static void ofdrm_device_set_gamma_linear(struct ofdrm_device *odev,
+					  const struct drm_format_info *format)
+{
+	struct drm_device *dev = &odev->dev;
+	int i;
+
+	switch (format->format) {
+	case DRM_FORMAT_RGB565:
+		/* Use better interpolation, to take 32 values from 0 to 255 */
+		for (i = 0; i < OFDRM_GAMMA_LUT_SIZE / 8; i++) {
+			unsigned char r = i * 8 + i / 4;
+			unsigned char g = i * 4 + i / 16;
+			unsigned char b = i * 8 + i / 4;
+
+			odev->funcs->cmap_write(odev, i, r, g, b);
+		}
+		/* Green has one more bit, so add padding with 0 for red and blue. */
+		for (i = OFDRM_GAMMA_LUT_SIZE / 8; i < OFDRM_GAMMA_LUT_SIZE / 4; i++) {
+			unsigned char r = 0;
+			unsigned char g = i * 4 + i / 16;
+			unsigned char b = 0;
+
+			odev->funcs->cmap_write(odev, i, r, g, b);
+		}
+		break;
+	case DRM_FORMAT_XRGB8888:
+		for (i = 0; i < OFDRM_GAMMA_LUT_SIZE; i++)
+			odev->funcs->cmap_write(odev, i, i, i, i);
+		break;
+	default:
+		drm_warn_once(dev, "Unsupported format %p4cc for gamma correction\n",
+			      &format->format);
+		break;
+	}
+}
+
+static void ofdrm_device_set_gamma(struct ofdrm_device *odev,
+				   const struct drm_format_info *format,
+				   struct drm_color_lut *lut)
+{
+	struct drm_device *dev = &odev->dev;
+	int i;
+
+	switch (format->format) {
+	case DRM_FORMAT_RGB565:
+		/* Use better interpolation, to take 32 values from lut[0] to lut[255] */
+		for (i = 0; i < OFDRM_GAMMA_LUT_SIZE / 8; i++) {
+			unsigned char r = lut[i * 8 + i / 4].red >> 8;
+			unsigned char g = lut[i * 4 + i / 16].green >> 8;
+			unsigned char b = lut[i * 8 + i / 4].blue >> 8;
+
+			odev->funcs->cmap_write(odev, i, r, g, b);
+		}
+		/* Green has one more bit, so add padding with 0 for red and blue. */
+		for (i = OFDRM_GAMMA_LUT_SIZE / 8; i < OFDRM_GAMMA_LUT_SIZE / 4; i++) {
+			unsigned char r = 0;
+			unsigned char g = lut[i * 4 + i / 16].green >> 8;
+			unsigned char b = 0;
+
+			odev->funcs->cmap_write(odev, i, r, g, b);
+		}
+		break;
+	case DRM_FORMAT_XRGB8888:
+		for (i = 0; i < OFDRM_GAMMA_LUT_SIZE; i++) {
+			unsigned char r = lut[i].red >> 8;
+			unsigned char g = lut[i].green >> 8;
+			unsigned char b = lut[i].blue >> 8;
+
+			odev->funcs->cmap_write(odev, i, r, g, b);
+		}
+		break;
+	default:
+		drm_warn_once(dev, "Unsupported format %p4cc for gamma correction\n",
+			      &format->format);
+		break;
+	}
+}
+
 /*
  * Modesetting
  */
 
 struct ofdrm_crtc_state {
 	struct drm_crtc_state base;
+
+	/* Primary-plane format; required for color mgmt. */
+	const struct drm_format_info *format;
 };
 
 static struct ofdrm_crtc_state *to_ofdrm_crtc_state(struct drm_crtc_state *base)

@@ -376,10 +735,12 @@ static int ofdrm_primary_plane_helper_atomic_check(struct drm_plane *plane,
 						   struct drm_atomic_state *new_state)
 {
 	struct drm_plane_state *new_plane_state = drm_atomic_get_new_plane_state(new_state, plane);
+	struct drm_framebuffer *new_fb = new_plane_state->fb;
 	struct drm_crtc_state *new_crtc_state;
+	struct ofdrm_crtc_state *new_ofdrm_crtc_state;
 	int ret;
 
-	if (!new_plane_state->fb)
+	if (!new_fb)
 		return 0;
 
 	new_crtc_state = drm_atomic_get_new_crtc_state(new_state, new_plane_state->crtc);

@@ -391,6 +752,14 @@ static int ofdrm_primary_plane_helper_atomic_check(struct drm_plane *plane,
 	if (ret)
 		return ret;
 
+	if (!new_plane_state->visible)
+		return 0;
+
+	new_crtc_state = drm_atomic_get_new_crtc_state(new_state, new_plane_state->crtc);
+
+	new_ofdrm_crtc_state = to_ofdrm_crtc_state(new_crtc_state);
+	new_ofdrm_crtc_state->format = new_fb->format;
+
 	return 0;
 }
 

@@ -435,6 +804,9 @@ static enum drm_mode_status ofdrm_crtc_helper_mode_valid(struct drm_crtc *crtc,
 static int ofdrm_crtc_helper_atomic_check(struct drm_crtc *crtc,
 					  struct drm_atomic_state *new_state)
 {
+	static const size_t gamma_lut_length = OFDRM_GAMMA_LUT_SIZE * sizeof(struct drm_color_lut);
+
+	struct drm_device *dev = crtc->dev;
 	struct drm_crtc_state *new_crtc_state = drm_atomic_get_new_crtc_state(new_state, crtc);
 	int ret;
 

@@ -442,9 +814,35 @@ static int ofdrm_crtc_helper_atomic_check(struct drm_crtc *crtc,
 	if (ret)
 		return ret;
 
+	if (new_crtc_state->color_mgmt_changed) {
+		struct drm_property_blob *gamma_lut = new_crtc_state->gamma_lut;
+
+		if (gamma_lut && (gamma_lut->length != gamma_lut_length)) {
+			drm_dbg(dev, "Incorrect gamma_lut length %zu\n", gamma_lut->length);
+			return -EINVAL;
+		}
+	}
+
 	return drm_atomic_add_affected_planes(new_state, crtc);
 }
 
+static void ofdrm_crtc_helper_atomic_flush(struct drm_crtc *crtc,
+					   struct drm_atomic_state *old_state)
+{
+	struct ofdrm_device *odev = ofdrm_device_of_dev(crtc->dev);
+	struct drm_crtc_state *crtc_state = crtc->state;
+	struct ofdrm_crtc_state *ofdrm_crtc_state = to_ofdrm_crtc_state(crtc_state);
+
+	if (crtc_state->enable && crtc_state->color_mgmt_changed) {
+		const struct drm_format_info *format = ofdrm_crtc_state->format;
+
+		if (crtc_state->gamma_lut)
+			ofdrm_device_set_gamma(odev, format, crtc_state->gamma_lut->data);
+		else
+			ofdrm_device_set_gamma_linear(odev, format);
+	}
+}
+
 static void ofdrm_crtc_helper_atomic_enable(struct drm_crtc *crtc,
 					    struct drm_atomic_state *old_state)
 {

@@ -466,6 +864,7 @@ static void ofdrm_crtc_helper_atomic_disable(struct drm_crtc *crtc,
 static const struct drm_crtc_helper_funcs ofdrm_crtc_helper_funcs = {
 	.mode_valid = ofdrm_crtc_helper_mode_valid,
 	.atomic_check = ofdrm_crtc_helper_atomic_check,
+	.atomic_flush = ofdrm_crtc_helper_atomic_flush,
 	.atomic_enable = ofdrm_crtc_helper_atomic_enable,
 	.atomic_disable = ofdrm_crtc_helper_atomic_disable,
 };

@@ -501,6 +900,7 @@ static struct drm_crtc_state *ofdrm_crtc_atomic_duplicate_state(struct drm_crtc
 		return NULL;
 
 	__drm_atomic_helper_crtc_duplicate_state(crtc, &new_ofdrm_crtc_state->base);
+	new_ofdrm_crtc_state->format = ofdrm_crtc_state->format;
 
 	return &new_ofdrm_crtc_state->base;
 }

@@ -553,27 +953,43 @@ static const struct ofdrm_device_funcs ofdrm_unknown_device_funcs = {
 };
 
 static const struct ofdrm_device_funcs ofdrm_mach64_device_funcs = {
+	.cmap_ioremap = ofdrm_mach64_cmap_ioremap,
+	.cmap_write = ofdrm_mach64_cmap_write,
 };
 
 static const struct ofdrm_device_funcs ofdrm_rage128_device_funcs = {
+	.cmap_ioremap = ofdrm_rage128_cmap_ioremap,
+	.cmap_write = ofdrm_rage128_cmap_write,
 };
 
 static const struct ofdrm_device_funcs ofdrm_rage_m3a_device_funcs = {
+	.cmap_ioremap = ofdrm_rage_m3a_cmap_ioremap,
+	.cmap_write = ofdrm_rage_m3a_cmap_write,
 };
 
 static const struct ofdrm_device_funcs ofdrm_rage_m3b_device_funcs = {
+	.cmap_ioremap = ofdrm_rage_m3b_cmap_ioremap,
+	.cmap_write = ofdrm_rage_m3b_cmap_write,
 };
 
 static const struct ofdrm_device_funcs ofdrm_radeon_device_funcs = {
+	.cmap_ioremap = ofdrm_radeon_cmap_ioremap,
+	.cmap_write = ofdrm_rage128_cmap_write, /* same as Rage128 */
 };
 
 static const struct ofdrm_device_funcs ofdrm_gxt2000_device_funcs = {
+	.cmap_ioremap = ofdrm_gxt2000_cmap_ioremap,
+	.cmap_write = ofdrm_gxt2000_cmap_write,
 };
 
 static const struct ofdrm_device_funcs ofdrm_avivo_device_funcs = {
+	.cmap_ioremap = ofdrm_avivo_cmap_ioremap,
+	.cmap_write = ofdrm_avivo_cmap_write,
 };
 
 static const struct ofdrm_device_funcs ofdrm_qemu_device_funcs = {
+	.cmap_ioremap = ofdrm_qemu_cmap_ioremap,
+	.cmap_write = ofdrm_qemu_cmap_write,
 };
 
 static struct ofdrm_device *ofdrm_device_create(struct drm_driver *drv,