cfb_console: Add support for some ANSI terminal escape codes

Add optional support for some ANSI escape sequences to the
cfb_console driver. Define CONFIG_CFB_CONSOLE_ANSI to enable
cursor moving, color reverting and clearing the cfb console
via ANSI escape codes.

Signed-off-by: Pali Rohár <pali.rohar@gmail.com>
Signed-off-by: Anatolij Gustschin <agust@denx.de>

1 files changed, 319 insertions, 9 deletions

diff --git a/drivers/video/cfb_console.c b/drivers/video/cfb_console.c
index 9f7794fe53..9c67b63bf4 100644
--- a/drivers/video/cfb_console.c
+++ b/drivers/video/cfb_console.c
@@ -385,6 +385,13 @@ static u32 eorx, fgx, bgx;	/* color pats */
 
 static int cfb_do_flush_cache;
 
+#ifdef CONFIG_CFB_CONSOLE_ANSI
+static char ansi_buf[10];
+static int ansi_buf_size;
+static int ansi_colors_need_revert;
+static int ansi_cursor_hidden;
+#endif
+
 static const int video_font_draw_table8[] = {
 	0x00000000, 0x000000ff, 0x0000ff00, 0x0000ffff,
 	0x00ff0000, 0x00ff00ff, 0x00ffff00, 0x00ffffff,
@@ -768,9 +775,97 @@ static void console_back(void)
 	}
 }
 
-static void console_newline(void)
+#ifdef CONFIG_CFB_CONSOLE_ANSI
+
+static void console_clear(void)
+{
+#ifdef VIDEO_HW_RECTFILL
+	video_hw_rectfill(VIDEO_PIXEL_SIZE,	/* bytes per pixel */
+			  0,			/* dest pos x */
+			  video_logo_height,	/* dest pos y */
+			  VIDEO_VISIBLE_COLS,	/* frame width */
+			  VIDEO_VISIBLE_ROWS,	/* frame height */
+			  bgx			/* fill color */
+	);
+#else
+	memsetl(CONSOLE_ROW_FIRST, CONSOLE_SIZE, bgx);
+#endif
+}
+
+static void console_cursor_fix(void)
+{
+	if (console_row < 0)
+		console_row = 0;
+	if (console_row >= CONSOLE_ROWS)
+		console_row = CONSOLE_ROWS - 1;
+	if (console_col < 0)
+		console_col = 0;
+	if (console_col >= CONSOLE_COLS)
+		console_col = CONSOLE_COLS - 1;
+}
+
+static void console_cursor_up(int n)
+{
+	console_row -= n;
+	console_cursor_fix();
+}
+
+static void console_cursor_down(int n)
+{
+	console_row += n;
+	console_cursor_fix();
+}
+
+static void console_cursor_left(int n)
+{
+	console_col -= n;
+	console_cursor_fix();
+}
+
+static void console_cursor_right(int n)
+{
+	console_col += n;
+	console_cursor_fix();
+}
+
+static void console_cursor_set_position(int row, int col)
+{
+	if (console_row != -1)
+		console_row = row;
+	if (console_col != -1)
+		console_col = col;
+	console_cursor_fix();
+}
+
+static void console_previousline(int n)
 {
-	console_row++;
+	/* FIXME: also scroll terminal ? */
+	console_row -= n;
+	console_cursor_fix();
+}
+
+static void console_swap_colors(void)
+{
+	eorx = fgx;
+	fgx = bgx;
+	bgx = eorx;
+	eorx = fgx ^ bgx;
+}
+
+static inline int console_cursor_is_visible(void)
+{
+	return !ansi_cursor_hidden;
+}
+#else
+static inline int console_cursor_is_visible(void)
+{
+	return 1;
+}
+#endif
+
+static void console_newline(int n)
+{
+	console_row += n;
 	console_col = 0;
 
 	/* Check if we need to scroll the terminal */
@@ -779,7 +874,7 @@ static void console_newline(void)
 		console_scrollup();
 
 		/* Decrement row number */
-		console_row--;
+		console_row = CONSOLE_ROWS - 1;
 	}
 }
 
@@ -788,11 +883,12 @@ static void console_cr(void)
 	console_col = 0;
 }
 
-void video_putc(const char c)
+static void parse_putc(const char c)
 {
 	static int nl = 1;
 
-	CURSOR_OFF;
+	if (console_cursor_is_visible())
+		CURSOR_OFF;
 
 	switch (c) {
 	case 13:		/* back to first column */
@@ -801,7 +897,7 @@ void video_putc(const char c)
 
 	case '\n':		/* next line */
 		if (console_col || (!console_col && nl))
-			console_newline();
+			console_newline(1);
 		nl = 1;
 		break;
 
@@ -810,7 +906,7 @@ void video_putc(const char c)
 		console_col &= ~0x0007;
 
 		if (console_col >= CONSOLE_COLS)
-			console_newline();
+			console_newline(1);
 		break;
 
 	case 8:		/* backspace */
@@ -827,11 +923,225 @@ void video_putc(const char c)
 
 		/* check for newline */
 		if (console_col >= CONSOLE_COLS) {
-			console_newline();
+			console_newline(1);
 			nl = 0;
 		}
 	}
-	CURSOR_SET;
+
+	if (console_cursor_is_visible())
+		CURSOR_SET;
+}
+
+void video_putc(const char c)
+{
+#ifdef CONFIG_CFB_CONSOLE_ANSI
+	int i;
+
+	if (c == 27) {
+		for (i = 0; i < ansi_buf_size; ++i)
+			parse_putc(ansi_buf[i]);
+		ansi_buf[0] = 27;
+		ansi_buf_size = 1;
+		return;
+	}
+
+	if (ansi_buf_size > 0) {
+		/*
+		 * 0 - ESC
+		 * 1 - [
+		 * 2 - num1
+		 * 3 - ..
+		 * 4 - ;
+		 * 5 - num2
+		 * 6 - ..
+		 * - cchar
+		 */
+		int next = 0;
+
+		int flush = 0;
+		int fail = 0;
+
+		int num1 = 0;
+		int num2 = 0;
+		int cchar = 0;
+
+		ansi_buf[ansi_buf_size++] = c;
+
+		if (ansi_buf_size >= sizeof(ansi_buf))
+			fail = 1;
+
+		for (i = 0; i < ansi_buf_size; ++i) {
+			if (fail)
+				break;
+
+			switch (next) {
+			case 0:
+				if (ansi_buf[i] == 27)
+					next = 1;
+				else
+					fail = 1;
+				break;
+
+			case 1:
+				if (ansi_buf[i] == '[')
+					next = 2;
+				else
+					fail = 1;
+				break;
+
+			case 2:
+				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
+					num1 = ansi_buf[i]-'0';
+					next = 3;
+				} else if (ansi_buf[i] != '?') {
+					--i;
+					num1 = 1;
+					next = 4;
+				}
+				break;
+
+			case 3:
+				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
+					num1 *= 10;
+					num1 += ansi_buf[i]-'0';
+				} else {
+					--i;
+					next = 4;
+				}
+				break;
+
+			case 4:
+				if (ansi_buf[i] != ';') {
+					--i;
+					next = 7;
+				} else
+					next = 5;
+				break;
+
+			case 5:
+				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
+					num2 = ansi_buf[i]-'0';
+					next = 6;
+				} else
+					fail = 1;
+				break;
+
+			case 6:
+				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
+					num2 *= 10;
+					num2 += ansi_buf[i]-'0';
+				} else {
+					--i;
+					next = 7;
+				}
+				break;
+
+			case 7:
+				if ((ansi_buf[i] >= 'A' && ansi_buf[i] <= 'H')
+					|| ansi_buf[i] == 'J'
+					|| ansi_buf[i] == 'K'
+					|| ansi_buf[i] == 'h'
+					|| ansi_buf[i] == 'l'
+					|| ansi_buf[i] == 'm') {
+					cchar = ansi_buf[i];
+					flush = 1;
+				} else
+					fail = 1;
+				break;
+			}
+		}
+
+		if (fail) {
+			for (i = 0; i < ansi_buf_size; ++i)
+				parse_putc(ansi_buf[i]);
+			ansi_buf_size = 0;
+			return;
+		}
+
+		if (flush) {
+			if (!ansi_cursor_hidden)
+				CURSOR_OFF;
+			ansi_buf_size = 0;
+			switch (cchar) {
+			case 'A':
+				/* move cursor num1 rows up */
+				console_cursor_up(num1);
+				break;
+			case 'B':
+				/* move cursor num1 rows down */
+				console_cursor_down(num1);
+				break;
+			case 'C':
+				/* move cursor num1 columns forward */
+				console_cursor_right(num1);
+				break;
+			case 'D':
+				/* move cursor num1 columns back */
+				console_cursor_left(num1);
+				break;
+			case 'E':
+				/* move cursor num1 rows up at begin of row */
+				console_previousline(num1);
+				break;
+			case 'F':
+				/* move cursor num1 rows down at begin of row */
+				console_newline(num1);
+				break;
+			case 'G':
+				/* move cursor to column num1 */
+				console_cursor_set_position(-1, num1-1);
+				break;
+			case 'H':
+				/* move cursor to row num1, column num2 */
+				console_cursor_set_position(num1-1, num2-1);
+				break;
+			case 'J':
+				/* clear console and move cursor to 0, 0 */
+				console_clear();
+				console_cursor_set_position(0, 0);
+				break;
+			case 'K':
+				/* clear line */
+				if (num1 == 0)
+					console_clear_line(console_row,
+							console_col,
+							CONSOLE_COLS-1);
+				else if (num1 == 1)
+					console_clear_line(console_row,
+							0, console_col);
+				else
+					console_clear_line(console_row,
+							0, CONSOLE_COLS-1);
+				break;
+			case 'h':
+				ansi_cursor_hidden = 0;
+				break;
+			case 'l':
+				ansi_cursor_hidden = 1;
+				break;
+			case 'm':
+				if (num1 == 0) { /* reset swapped colors */
+					if (ansi_colors_need_revert) {
+						console_swap_colors();
+						ansi_colors_need_revert = 0;
+					}
+				} else if (num1 == 7) { /* once swap colors */
+					if (!ansi_colors_need_revert) {
+						console_swap_colors();
+						ansi_colors_need_revert = 1;
+					}
+				}
+				break;
+			}
+			if (!ansi_cursor_hidden)
+				CURSOR_SET;
+		}
+	} else {
+		parse_putc(c);
+	}
+#else
+	parse_putc(c);
+#endif
 }
 
 void video_puts(const char *s)