dm: video: support more escape sequences

The EFI subsystems needs to know the size of the terminal. If the
environment variable

	stdout = serial,vidconsole

this size cannot be read from the video console. So the EFI subsystem
sends escape sequences to read the size. With this patch we get support
for the following escape sequences:

ESC "7"		Save cursor position
ESC "8"		Restore cursor position

Signed-off-by: Heinrich Schuchardt <xypron.glpk@gmx.de>

2 files changed, 78 insertions, 25 deletions

diff --git a/drivers/video/vidconsole-uclass.c b/drivers/video/vidconsole-uclass.c
index 0c36a5de0a..7f95e9c6e5 100644
--- a/drivers/video/vidconsole-uclass.c
+++ b/drivers/video/vidconsole-uclass.c
@@ -165,6 +165,43 @@ static char *parsenum(char *s, int *num)
 	return end;
 }
 
+/**
+ * set_cursor_position() - set cursor position
+ *
+ * @priv:	private data of the video console
+ * @row:	new row
+ * @col:	new column
+ */
+static void set_cursor_position(struct vidconsole_priv *priv, int row, int col)
+{
+	/*
+	 * Ensure we stay in the bounds of the screen.
+	 */
+	if (row >= priv->rows)
+		row = priv->rows - 1;
+	if (col >= priv->cols)
+		col = priv->cols - 1;
+
+	priv->ycur = row * priv->y_charsize;
+	priv->xcur_frac = priv->xstart_frac +
+			  VID_TO_POS(col * priv->x_charsize);
+}
+
+/**
+ * get_cursor_position() - get cursor position
+ *
+ * @priv:	private data of the video console
+ * @row:	row
+ * @col:	column
+ */
+static void get_cursor_position(struct vidconsole_priv *priv,
+				int *row, int *col)
+{
+	*row = priv->ycur / priv->y_charsize;
+	*col = VID_TO_PIXEL(priv->xcur_frac - priv->xstart_frac) /
+	       priv->x_charsize;
+}
+
 /*
  * Process a character while accumulating an escape string.  Chars are
  * accumulated into escape_buf until the end of escape sequence is
@@ -180,8 +217,30 @@ static void vidconsole_escape_char(struct udevice *dev, char ch)
 	/* Sanity checking for bogus ESC sequences: */
 	if (priv->escape_len >= sizeof(priv->escape_buf))
 		goto error;
-	if (priv->escape_len == 0 && ch != '[')
-		goto error;
+	if (priv->escape_len == 0) {
+		switch (ch) {
+		case '7':
+			/* Save cursor position */
+			get_cursor_position(priv, &priv->row_saved,
+					    &priv->col_saved);
+			priv->escape = 0;
+
+			return;
+		case '8': {
+			/* Restore cursor position */
+			int row = priv->row_saved;
+			int col = priv->col_saved;
+
+			set_cursor_position(priv, row, col);
+			priv->escape = 0;
+			return;
+		}
+		case '[':
+			break;
+		default:
+			goto error;
+		}
+	}
 
 	priv->escape_buf[priv->escape_len++] = ch;
 
@@ -213,17 +272,7 @@ static void vidconsole_escape_char(struct udevice *dev, char ch)
 		s++;    /* ; */
 		s = parsenum(s, &col);
 
-		/*
-		 * Ensure we stay in the bounds of the screen.
-		 */
-		if (row >= priv->rows)
-			row = priv->rows - 1;
-		if (col >= priv->cols)
-			col = priv->cols - 1;
-
-		priv->ycur = row * priv->y_charsize;
-		priv->xcur_frac = priv->xstart_frac +
-			VID_TO_POS(col * priv->x_charsize);
+		set_cursor_position(priv, row, col);
 
 		break;
 	}
diff --git a/include/video_console.h b/include/video_console.h
index 44e7bdbe29..52a41ac200 100644
--- a/include/video_console.h
+++ b/include/video_console.h
@@ -43,20 +43,22 @@ enum color_idx {
  * Drivers must set up @rows, @cols, @x_charsize, @y_charsize in their probe()
  * method. Drivers may set up @xstart_frac if desired.
  *
- * @sdev:	stdio device, acting as an output sink
- * @xcur_frac:	Current X position, in fractional units (VID_TO_POS(x))
- * @ycur:	Current Y position in pixels (0=top)
- * @rows:	Number of text rows
- * @cols:	Number of text columns
- * @x_charsize:	Character width in pixels
- * @y_charsize:	Character height in pixels
+ * @sdev:		stdio device, acting as an output sink
+ * @xcur_frac:		Current X position, in fractional units (VID_TO_POS(x))
+ * @ycur:		Current Y position in pixels (0=top)
+ * @rows:		Number of text rows
+ * @cols:		Number of text columns
+ * @x_charsize:		Character width in pixels
+ * @y_charsize:		Character height in pixels
  * @tab_width_frac:	Tab width in fractional units
- * @xsize_frac:	Width of the display in fractional units
+ * @xsize_frac:		Width of the display in fractional units
  * @xstart_frac:	Left margin for the text console in fractional units
- * @last_ch:	Last character written to the text console on this line
- * @escape:	TRUE if currently accumulating an ANSI escape sequence
- * @escape_len:	Length of accumulated escape sequence so far
- * @escape_buf:	Buffer to accumulate escape sequence
+ * @last_ch:		Last character written to the text console on this line
+ * @escape:		TRUE if currently accumulating an ANSI escape sequence
+ * @escape_len:		Length of accumulated escape sequence so far
+ * @col_saved:		Saved X position, in fractional units (VID_TO_POS(x))
+ * @row_saved:		Saved Y position in pixels (0=top)
+ * @escape_buf:		Buffer to accumulate escape sequence
  */
 struct vidconsole_priv {
 	struct stdio_dev sdev;
@@ -77,6 +79,8 @@ struct vidconsole_priv {
 	 */
 	int escape;
 	int escape_len;
+	int row_saved;
+	int col_saved;
 	char escape_buf[32];
 };