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]; };