[PATCH v2 09/11] fblog: register console driver

[David Herrmann <dh.herrmann@googlemail.com>]

We want to print the kernel log to all FBs so we need a console driver.
This registers the driver on startup and writes all messages to all
registered fblog instances.

We cannot share a console buffer between FBs because they might have
different resolutions. Therefore, we create one buffer per object. We
destroy the buffer during close() so we do not waste memory if it is not
used.

Signed-off-by: David Herrmann <dh.herrmann@googlemail.com>
---
 drivers/video/console/fblog.c | 150 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 150 insertions(+)

diff --git a/drivers/video/console/fblog.c b/drivers/video/console/fblog.c
index fd1f3d6..e447f98 100644
--- a/drivers/video/console/fblog.c
+++ b/drivers/video/console/fblog.c
@@ -23,11 +23,34 @@
  * all fblog instances before running other graphics applications.
  */
 
+#include <linux/console.h>
 #include <linux/device.h>
 #include <linux/fb.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
 
+/**
+ * struct fblog_buf: Console text buffer
+ *
+ * Each framebuffer has its own text buffer which contains all characters that
+ * are currently printed on screen. The buffers might have different sizes and
+ * can be resized during runtime. When the buffer content changes, we redraw the
+ * screen.
+ *
+ * width: Width of buffer in characters
+ * height: Height of buffer in characters
+ * lines: Array of lines
+ * pos_x: Cursor x-position
+ * pos_y: Cursor y-position
+ */
+struct fblog_buf {
+	size_t width;
+	size_t height;
+	u16 **lines;
+	size_t pos_x;
+	size_t pos_y;
+};
+
 enum fblog_flags {
 	FBLOG_KILLED,
 	FBLOG_OPEN,
@@ -40,6 +63,7 @@ struct fblog_fb {
 	struct fb_info *info;
 	struct device dev;
 	struct mutex lock;
+	struct fblog_buf buf;
 };
 
 static DEFINE_MUTEX(fblog_registration_lock);
@@ -47,6 +71,107 @@ static struct fblog_fb *fblog_fbs[FB_MAX];
 static bool active = 1;
 
 #define to_fblog_dev(_d) container_of(_d, struct fblog_fb, dev)
+#define FBLOG_STR(x) x, sizeof(x) - 1
+
+static void fblog_buf_resize(struct fblog_buf *buf, size_t width,
+			     size_t height)
+{
+	u16 **lines = NULL;
+	size_t i, j, minw, minh;
+
+	if (buf->height = height && buf->width = width)
+		return;
+
+	if (width && height) {
+		lines = kzalloc(height * sizeof(char*), GFP_KERNEL);
+		if (!lines)
+			return;
+
+		for (i = 0; i < height; ++i) {
+			lines[i] = kzalloc(width * sizeof(u16), GFP_KERNEL);
+			if (!lines[i]) {
+				while (i--)
+					kfree(lines[i]);
+				return;
+			}
+		}
+
+		/* copy old lines */
+		minw = min(width, buf->width);
+		minh = min(height, buf->height);
+		if (height >= buf->height)
+			i = 0;
+		else
+			i = buf->height - height;
+
+		for (j = 0; j < minh; ++i, ++j)
+			memcpy(lines[j], buf->lines[i], minw * sizeof(u16));
+	} else {
+		width = 0;
+		height = 0;
+	}
+
+	for (i = 0; i < buf->height; ++i)
+		kfree(buf->lines[i]);
+	kfree(buf->lines);
+
+	buf->lines = lines;
+	buf->width = width;
+	buf->height = height;
+}
+
+static void fblog_buf_deinit(struct fblog_buf *buf)
+{
+	fblog_buf_resize(buf, 0, 0);
+}
+
+static void fblog_buf_rotate(struct fblog_buf *buf)
+{
+	u16 *line;
+
+	if (!buf->height)
+		return;
+
+	line = buf->lines[0];
+	memset(line, 0, sizeof(u16) * buf->width);
+
+	memmove(buf->lines, &buf->lines[1], sizeof(char*) * (buf->height - 1));
+	buf->lines[buf->height - 1] = line;
+}
+
+static void fblog_buf_write(struct fblog_buf *buf, const char *str, size_t len)
+{
+	char c;
+
+	if (!buf->height)
+		return;
+
+	while (len--) {
+		c = *str++;
+
+		if (c = 0)
+			c = '?';
+
+		if (c = '\n') {
+			buf->pos_x = 0;
+			if (++buf->pos_y >= buf->height) {
+				buf->pos_y = buf->height - 1;
+				fblog_buf_rotate(buf);
+			}
+		} else {
+			if (buf->pos_x >= buf->width) {
+				buf->pos_x = 0;
+				++buf->pos_y;
+			}
+			if (buf->pos_y >= buf->height) {
+				buf->pos_y = buf->height - 1;
+				fblog_buf_rotate(buf);
+			}
+
+			buf->lines[buf->pos_y][buf->pos_x++] = c;
+		}
+	}
+}
 
 static int fblog_open(struct fblog_fb *fb, bool locked)
 {
@@ -80,6 +205,8 @@ static int fblog_open(struct fblog_fb *fb, bool locked)
 	if (!locked)
 		mutex_unlock(&fb->info->lock);
 
+	fblog_buf_resize(&fb->buf, 80, 24);
+	fblog_buf_write(&fb->buf, FBLOG_STR("Framebuffer log initialized\n"));
 	set_bit(FBLOG_OPEN, &fb->flags);
 	mutex_unlock(&fb->lock);
 	return 0;
@@ -111,6 +238,7 @@ static void fblog_close(struct fblog_fb *fb, bool kill_dev, bool locked)
 			mutex_unlock(&fb->info->lock);
 
 		clear_bit(FBLOG_OPEN, &fb->flags);
+		fblog_buf_deinit(&fb->buf);
 	}
 
 	if (kill_dev)
@@ -368,6 +496,26 @@ static struct notifier_block fblog_notifier = {
 	.notifier_call = fblog_event,
 };
 
+static void fblog_con_write(struct console *con, const char *buf,
+			    unsigned int len)
+{
+	int i;
+
+	mutex_lock(&fblog_registration_lock);
+	for (i = 0; i < FB_MAX; ++i) {
+		if (fblog_fbs[i]) {
+			fblog_buf_write(&fblog_fbs[i]->buf, buf, len);
+		}
+	}
+	mutex_unlock(&fblog_registration_lock);
+}
+
+static struct console fblog_con_driver = {
+	.name = "fblog",
+	.write = fblog_con_write,
+	.flags = CON_PRINTBUFFER | CON_ENABLED,
+};
+
 static int __init fblog_init(void)
 {
 	int ret;
@@ -379,6 +527,7 @@ static int __init fblog_init(void)
 	}
 
 	fblog_scan();
+	register_console(&fblog_con_driver);
 
 	return 0;
 }
@@ -388,6 +537,7 @@ static void __exit fblog_exit(void)
 	unsigned int i;
 	struct fb_info *info;
 
+	unregister_console(&fblog_con_driver);
 	fb_unregister_client(&fblog_notifier);
 
 	for (i = 0; i < FB_MAX; ++i) {
-- 
1.7.11.1