video: set up the kernel's simple framebuffer driver

Add support to configure the active framebuffer for the kernel through
device tree.

Signed-off-by: Andre Heider <a.heider@gmail.com>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

4 files changed, 182 insertions, 0 deletions

diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index b532e7d585..5539266226 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -77,4 +77,11 @@ config DRIVER_VIDEO_BCM2835
 	help
 	  Add support for the BCM2835/VideoCore frame buffer device.
 
+config DRIVER_VIDEO_SIMPLEFB
+	bool "Simple framebuffer support"
+	depends on OFTREE
+	help
+	  Add support for setting up the kernel's simple framebuffer driver
+	  based on the active barebox framebuffer.
+
 endif
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 244feab270..31edfca2b0 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -10,3 +10,4 @@ obj-$(CONFIG_DRIVER_VIDEO_PXA) += pxa.o
 obj-$(CONFIG_DRIVER_VIDEO_SDL) += sdl.o
 obj-$(CONFIG_DRIVER_VIDEO_OMAP) += omap.o
 obj-$(CONFIG_DRIVER_VIDEO_BCM2835) += bcm2835.o
+obj-$(CONFIG_DRIVER_VIDEO_SIMPLEFB) += simplefb.o
diff --git a/drivers/video/fb.c b/drivers/video/fb.c
index 0159994d13..4263027bf7 100644
--- a/drivers/video/fb.c
+++ b/drivers/video/fb.c
@@ -163,6 +163,13 @@ int register_framebuffer(struct fb_info *info)
 	if (ret)
 		goto err_unregister;
 
+	if (IS_ENABLED(CONFIG_DRIVER_VIDEO_SIMPLEFB)) {
+		ret = fb_register_simplefb(info);
+		if (ret)
+			dev_err(&info->dev, "failed to register simplefb: %s\n",
+					strerror(-ret));
+	}
+
 	return 0;
 
 err_unregister:
diff --git a/drivers/video/simplefb.c b/drivers/video/simplefb.c
new file mode 100644
index 0000000000..834704b21a
--- /dev/null
+++ b/drivers/video/simplefb.c
@@ -0,0 +1,167 @@
+/*
+ * SimpleFB driver
+ *
+ * Copyright (C) 2013 Andre Heider
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#define pr_fmt(fmt) "simplefb: " fmt
+
+#include <common.h>
+#include <errno.h>
+#include <fb.h>
+#include <fcntl.h>
+#include <fs.h>
+#include <init.h>
+#include <xfuncs.h>
+
+struct simplefb_mode {
+	const char *format;
+	u32 bpp;
+	struct fb_bitfield red;
+	struct fb_bitfield green;
+	struct fb_bitfield blue;
+	struct fb_bitfield transp;
+};
+
+/*
+ * These values have to match the kernel's simplefb driver.
+ * See Documentation/devicetree/bindings/video/simple-framebuffer.txt
+ */
+static const struct simplefb_mode simplefb_modes[] = {
+	{
+		.format	= "r5g6b5",
+		.bpp	= 16,
+		.red	= { .length = 5, .offset = 11 },
+		.green	= { .length = 6, .offset = 5 },
+		.blue	= { .length = 5, .offset = 0 },
+		.transp	= { .length = 0, .offset = 0 },
+	},
+};
+
+static bool simplefb_bitfield_cmp(const struct fb_bitfield *a,
+					const struct fb_bitfield *b)
+{
+	if (a->offset != b->offset)
+		return false;
+	if (a->length != b->length)
+		return false;
+	if (a->msb_right != b->msb_right)
+		return false;
+
+	return true;
+}
+
+static const struct simplefb_mode *simplefb_find_mode(const struct fb_info *fbi)
+{
+	const struct simplefb_mode *mode;
+	u32 i;
+
+	for (i = 0; i < ARRAY_SIZE(simplefb_modes); ++i) {
+		mode = &simplefb_modes[i];
+
+		if (fbi->bits_per_pixel != mode->bpp)
+			continue;
+		if (!simplefb_bitfield_cmp(&fbi->red, &mode->red))
+			continue;
+		if (!simplefb_bitfield_cmp(&fbi->green, &mode->green))
+			continue;
+		if (!simplefb_bitfield_cmp(&fbi->blue, &mode->blue))
+			continue;
+		if (!simplefb_bitfield_cmp(&fbi->transp, &mode->transp))
+			continue;
+
+		return mode;
+	}
+
+	return NULL;
+}
+
+static int simplefb_create_node(struct device_node *root,
+				const struct fb_info *fbi, const char *format)
+{
+	const char *compat = "simple-framebuffer";
+	const char *disabled = "disabled";
+	const char *okay = "okay";
+	struct device_node *node;
+	u32 cells[2];
+	int ret;
+
+	node = of_create_node(root, "/framebuffer");
+	if (!node)
+		return -ENOMEM;
+
+	ret = of_set_property(node, "status", disabled,
+				strlen(disabled) + 1, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = of_set_property(node, "compatible", compat, strlen(compat) + 1, 1);
+	if (ret)
+		return ret;
+
+	cells[0] = cpu_to_be32((u32)fbi->screen_base);
+	cells[1] = cpu_to_be32(fbi->line_length * fbi->yres);
+	ret = of_set_property(node, "reg", cells, sizeof(cells[0]) * 2, 1);
+	if (ret < 0)
+		return ret;
+
+	cells[0] = cpu_to_be32(fbi->xres);
+	ret = of_set_property(node, "width", cells, sizeof(cells[0]), 1);
+	if (ret < 0)
+		return ret;
+
+	cells[0] = cpu_to_be32(fbi->yres);
+	ret = of_set_property(node, "height", cells, sizeof(cells[0]), 1);
+	if (ret < 0)
+		return ret;
+
+	cells[0] = cpu_to_be32(fbi->line_length);
+	ret = of_set_property(node, "stride", cells, sizeof(cells[0]), 1);
+	if (ret < 0)
+		return ret;
+
+	ret = of_set_property(node, "format", format, strlen(format) + 1, 1);
+	if (ret < 0)
+		return ret;
+
+	return of_set_property(node, "status", okay, strlen(okay) + 1, 1);
+}
+
+static int simplefb_of_fixup(struct device_node *root, void *ctx)
+{
+	struct fb_info *info = ctx;
+	const struct simplefb_mode *mode;
+	int ret;
+
+	/* only create node if we are requested to */
+	if (!info->register_simplefb)
+		return 0;
+
+	/* do not create node for disabled framebuffers */
+	if (!info->enabled)
+		return 0;
+
+	mode = simplefb_find_mode(info);
+	if (!mode) {
+		dev_err(&info->dev, "fb format is incompatible with simplefb\n");
+		return -EINVAL;
+	}
+
+	ret = simplefb_create_node(root, info, mode->format);
+	if (ret)
+		dev_err(&info->dev, "failed to create node: %d\n", ret);
+	else
+		dev_info(&info->dev, "created %s node\n", mode->format);
+
+	return ret;
+}
+
+int fb_register_simplefb(struct fb_info *info)
+{
+	dev_add_param_bool(&info->dev, "register_simplefb",
+			NULL, NULL, &info->register_simplefb, NULL);
+
+	return of_register_fixup(simplefb_of_fixup, info);
+}