From 72e98228c3f17a5411b4f8fea27cfd6072ace348 Mon Sep 17 00:00:00 2001
From: Simon Glass <sjg@chromium.org>
Date: Thu, 4 Sep 2014 16:27:28 -0600
Subject: [PATCH] sandbox: serial: Support a coloured console

The current sandbox serial driver is a pretty trivial example and does not
have the featues that might be needed for other board serial drivers. To
help provide a better example, add a text colour property to the device
tree for sandbox. This uses platform data, a device tree node, driver
private data and a remove() method.

Signed-off-by: Simon Glass <sjg@chromium.org>
---
 drivers/serial/sandbox.c | 83 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 83 insertions(+)

diff --git a/drivers/serial/sandbox.c b/drivers/serial/sandbox.c
index ac54e0167e..cd2f91e28e 100644
--- a/drivers/serial/sandbox.c
+++ b/drivers/serial/sandbox.c
@@ -34,19 +34,67 @@ static char serial_buf[16];
 static unsigned int serial_buf_write;
 static unsigned int serial_buf_read;
 
+struct sandbox_serial_platdata {
+	int colour;	/* Text colour to use for output, -1 for none */
+};
+
+struct sandbox_serial_priv {
+	bool start_of_line;
+};
+
+/**
+ * output_ansi_colour() - Output an ANSI colour code
+ *
+ * @colour: Colour to output (0-7)
+ */
+static void output_ansi_colour(int colour)
+{
+	char ansi_code[] = "\x1b[1;3Xm";
+
+	ansi_code[5] = '0' + colour;
+	os_write(1, ansi_code, sizeof(ansi_code) - 1);
+}
+
+static void output_ansi_reset(void)
+{
+	os_write(1, "\x1b[0m", 4);
+}
+
 static int sandbox_serial_probe(struct udevice *dev)
 {
 	struct sandbox_state *state = state_get_current();
+	struct sandbox_serial_priv *priv = dev_get_priv(dev);
 
 	if (state->term_raw != STATE_TERM_COOKED)
 		os_tty_raw(0, state->term_raw == STATE_TERM_RAW_WITH_SIGS);
+	priv->start_of_line = 0;
+
+	return 0;
+}
+
+static int sandbox_serial_remove(struct udevice *dev)
+{
+	struct sandbox_serial_platdata *plat = dev->platdata;
+
+	if (plat->colour != -1)
+		output_ansi_reset();
 
 	return 0;
 }
 
 static int sandbox_serial_putc(struct udevice *dev, const char ch)
 {
+	struct sandbox_serial_priv *priv = dev_get_priv(dev);
+	struct sandbox_serial_platdata *plat = dev->platdata;
+
+	if (priv->start_of_line && plat->colour != -1) {
+		priv->start_of_line = false;
+		output_ansi_colour(plat->colour);
+	}
+
 	os_write(1, &ch, 1);
+	if (ch == '\n')
+		priv->start_of_line = true;
 
 	return 0;
 }
@@ -91,6 +139,32 @@ static int sandbox_serial_getc(struct udevice *dev)
 	return result;
 }
 
+static const char * const ansi_colour[] = {
+	"black", "red", "green", "yellow", "blue", "megenta", "cyan",
+	"white",
+};
+
+static int sandbox_serial_ofdata_to_platdata(struct udevice *dev)
+{
+	struct sandbox_serial_platdata *plat = dev->platdata;
+	const char *colour;
+	int i;
+
+	plat->colour = -1;
+	colour = fdt_getprop(gd->fdt_blob, dev->of_offset,
+			     "sandbox,text-colour", NULL);
+	if (colour) {
+		for (i = 0; i < ARRAY_SIZE(ansi_colour); i++) {
+			if (!strcmp(colour, ansi_colour[i])) {
+				plat->colour = i;
+				break;
+			}
+		}
+	}
+
+	return 0;
+}
+
 static const struct dm_serial_ops sandbox_serial_ops = {
 	.putc = sandbox_serial_putc,
 	.pending = sandbox_serial_pending,
@@ -106,11 +180,20 @@ U_BOOT_DRIVER(serial_sandbox) = {
 	.name	= "serial_sandbox",
 	.id	= UCLASS_SERIAL,
 	.of_match = sandbox_serial_ids,
+	.ofdata_to_platdata = sandbox_serial_ofdata_to_platdata,
+	.platdata_auto_alloc_size = sizeof(struct sandbox_serial_platdata),
+	.priv_auto_alloc_size = sizeof(struct sandbox_serial_priv),
 	.probe = sandbox_serial_probe,
+	.remove = sandbox_serial_remove,
 	.ops	= &sandbox_serial_ops,
 	.flags = DM_FLAG_PRE_RELOC,
 };
 
+static const struct sandbox_serial_platdata platdata_non_fdt = {
+	.colour = -1,
+};
+
 U_BOOT_DEVICE(serial_sandbox_non_fdt) = {
 	.name = "serial_sandbox",
+	.platdata = &platdata_non_fdt,
 };
-- 
2.39.5