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>

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 != '[')
+	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;
 	}

include/video_console.h

@@ -56,6 +56,8 @@ enum color_idx {
  * @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 {
@@ -77,6 +79,8 @@ struct vidconsole_priv {
 	 */
 	int escape;
 	int escape_len;
+	int row_saved;
+	int col_saved;
 	char escape_buf[32];
 };