>From f315c78469d556a4831eaabd5c911274be86afb3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pali=20Roh=C3=A1r?= <pali.rohar@gmail.com>
Date: Wed, 31 Aug 2011 15:41:24 +0200
Subject: [PATCH 11/15] drivers/video/cfb_console.c: Added support for ANSI escape codes

---
 drivers/video/cfb_console.c |  387 ++++++++++++++++++++++++++++++++++++++++---
 1 files changed, 361 insertions(+), 26 deletions(-)

diff --git a/drivers/video/cfb_console.c b/drivers/video/cfb_console.c
index 3a93b64..e16c76a 100644
--- a/drivers/video/cfb_console.c
+++ b/drivers/video/cfb_console.c
@@ -371,6 +371,10 @@ static int console_row;		/* cursor row */
 
 static u32 eorx, fgx, bgx;	/* color pats */
 
+static char ansi_buf[10] = { 0, };
+static int ansi_buf_size = 0;
+static int ansi_colors_need_revert = 0;
+
 static const int video_font_draw_table8[] = {
 	0x00000000, 0x000000ff, 0x0000ff00, 0x0000ffff,
 	0x00ff0000, 0x00ff00ff, 0x00ffff00, 0x00ffffff,
@@ -597,22 +601,34 @@ static void video_putchar(int xx, int yy, unsigned char c)
 	video_drawchars(xx, yy + video_logo_height, &c, 1);
 }
 
-#if defined(CONFIG_CONSOLE_CURSOR) || defined(CONFIG_VIDEO_SW_CURSOR)
-static void video_set_cursor(void)
+static void console_swap_colors(void)
 {
-	/* swap drawing colors */
 	eorx = fgx;
 	fgx = bgx;
 	bgx = eorx;
 	eorx = fgx ^ bgx;
+}
+
+static void console_set_text_color(int c)
+{
+	// TODO
+}
+
+static void console_set_background_color(int c)
+{
+	// TODO
+}
+
+#if defined(CONFIG_CONSOLE_CURSOR) || defined(CONFIG_VIDEO_SW_CURSOR)
+static void video_set_cursor(void)
+{
+	/* swap drawing colors */
+	console_swap_colors();
 	/* draw cursor */
 	video_putchar(console_col * VIDEO_FONT_WIDTH,
 		      console_row * VIDEO_FONT_HEIGHT, ' ');
 	/* restore drawing colors */
-	eorx = fgx;
-	fgx = bgx;
-	bgx = eorx;
-	eorx = fgx ^ bgx;
+	console_swap_colors();
 }
 #endif
 
@@ -672,6 +688,49 @@ static void memcpyl(int *d, int *s, int c)
 }
 #endif
 
+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_clear_line(int line, int begin, int end)
+{
+#ifdef VIDEO_HW_RECTFILL
+	video_hw_rectfill(VIDEO_PIXEL_SIZE,				/* bytes per pixel */
+			  VIDEO_FONT_WIDTH * begin,			/* dest pos x */  /* FIXME: correct? */
+			  video_logo_height + CONSOLE_ROW_SIZE * line,	/* dest pos y */  /* FIXME: correct? */
+			  VIDEO_FONT_WIDTH * ( end - begin ),		/* frame width */ /* FIXME: correct? */
+			  VIDEO_FONT_HEIGHT,				/* frame height */
+			  bgx						/* fill color */
+		);
+#else
+	int i;
+	if ( begin == 0 && end == CONSOLE_COLS )
+		memsetl(CONSOLE_ROW_FIRST + CONSOLE_ROW_SIZE * line,	/* offset of row */
+			CONSOLE_ROW_SIZE >> 2,				/* length of row */
+			bgx						/* fill color */
+		);
+	else
+		for ( i = 0; i < VIDEO_FONT_HEIGHT; ++i )
+			memsetl(CONSOLE_ROW_FIRST + CONSOLE_ROW_SIZE * line +	/* offset of row */
+				VIDEO_FONT_WIDTH * VIDEO_PIXEL_SIZE * begin +	/* offset of col */
+				i * VIDEO_LINE_LEN,				/* col offset of i-th line */
+				VIDEO_FONT_WIDTH * ( end - begin ) / 2,		/* length to end of line */
+				bgx						/* fill color */
+				);
+#endif
+}
+
 static void console_scrollup(void)
 {
 	/* copy up rows ignoring the first one */
@@ -694,18 +753,7 @@ static void console_scrollup(void)
 #endif
 
 	/* clear the last one */
-#ifdef VIDEO_HW_RECTFILL
-	video_hw_rectfill(VIDEO_PIXEL_SIZE,	/* bytes per pixel */
-			  0,			/* dest pos x */
-			  VIDEO_VISIBLE_ROWS
-			  - VIDEO_FONT_HEIGHT,	/* dest pos y */
-			  VIDEO_VISIBLE_COLS,	/* frame width */
-			  VIDEO_FONT_HEIGHT,	/* frame height */
-			  CONSOLE_BG_COL	/* fill color */
-		);
-#else
-	memsetl(CONSOLE_ROW_LAST, CONSOLE_ROW_SIZE >> 2, CONSOLE_BG_COL);
-#endif
+	console_clear_line(CONSOLE_ROWS-1, 0, CONSOLE_COLS);
 }
 
 static void console_back(void)
@@ -723,7 +771,7 @@ static void console_back(void)
 		      console_row * VIDEO_FONT_HEIGHT, ' ');
 }
 
-static void console_newline(void)
+static void console_cursor_check(void)
 {
 	/* Check if last character in the line was just drawn. If so, cursor was
 	   overwriten and need not to be cleared. Cursor clearing without this
@@ -732,7 +780,62 @@ static void console_newline(void)
 	 */
 	if (console_col < CONSOLE_COLS)
 		CURSOR_OFF;
-	console_row++;
+}
+
+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;
+}
+
+static void console_cursor_up(int n)
+{
+	console_cursor_check();
+	console_row -= n;
+	console_cursor_fix();
+}
+
+static void console_cursor_down(int n)
+{
+	console_cursor_check();
+	console_row += n;
+	console_cursor_fix();
+}
+
+static void console_cursor_left(int n)
+{
+	console_cursor_check();
+	console_col -= n;
+	console_cursor_fix();
+}
+
+static void console_cursor_right(int n)
+{
+	console_cursor_check();
+	console_col += n;
+	console_cursor_fix();
+}
+
+static void console_cursor_set_position(int row, int col)
+{
+	console_cursor_check();
+	if (console_row != -1)
+		console_row = row;
+	if (console_col != -1)
+		console_col = col;
+	console_cursor_fix();
+}
+
+static void console_newline(int n)
+{
+	console_cursor_check();
+	console_row += n;
 	console_col = 0;
 
 	/* Check if we need to scroll the terminal */
@@ -741,17 +844,25 @@ static void console_newline(void)
 		console_scrollup();
 
 		/* Decrement row number */
-		console_row--;
+		console_row = CONSOLE_ROWS-1;
 	}
 }
 
+static void console_previewsline(int n)
+{
+	/* FIXME: also scroll terminal ? */
+	console_cursor_check();
+	console_row -= n;
+	console_cursor_fix();
+}
+
 static void console_cr(void)
 {
 	CURSOR_OFF;
 	console_col = 0;
 }
 
-void video_putc(const char c)
+static void parse_putc(const char c)
 {
 	static int nl = 1;
 
@@ -762,7 +873,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;
 
@@ -772,7 +883,7 @@ void video_putc(const char c)
 		console_col &= ~0x0007;
 
 		if (console_col >= CONSOLE_COLS)
-			console_newline();
+			console_newline(1);
 		break;
 
 	case 8:		/* backspace */
@@ -786,13 +897,237 @@ void video_putc(const char c)
 
 		/* check for newline */
 		if (console_col >= CONSOLE_COLS) {
-			console_newline();
+			console_newline(1);
 			nl = 0;
 		}
 	}
 	CURSOR_SET;
 }
 
+void video_putc(const char c)
+{
+	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 - ..
+		// 7 - 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
+					fail = 1;
+				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] == '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)
+		{
+			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_previewsline(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 'm':
+				if (num1 == 0) //reset swapped colors
+				{
+					if (ansi_colors_need_revert)
+					{
+						console_swap_colors();
+						ansi_colors_need_revert = 0;
+					}
+				}
+				else if (num1 == 7) //swap colors (only once)
+				{
+					if (!ansi_colors_need_revert)
+					{
+						console_swap_colors();
+						ansi_colors_need_revert = 1;
+					}
+				}
+				else if (num1 == 38) // set default text color
+				{
+					fgx = CONSOLE_FG_COL;
+					eorx = fgx ^ bgx;
+				}
+				else if (num1 == 48) // set default background color
+				{
+					bgx = CONSOLE_BG_COL;
+					eorx = fgx ^ bgx;
+				}
+				else if (num1 >= 30 && num1 <= 37) //set text color
+					console_set_text_color(num1-30);
+				else if (num1 >= 40 && num1 <= 37) //set background color
+					console_set_background_color(num1-40);
+				break;
+			}
+		}
+	}
+	else
+	{
+		parse_putc(c);
+	}
+}
+
 void video_puts(const char *s)
 {
 	int count = strlen(s);
-- 
1.7.4.1

