[J-u-boot.git] / drivers / video / vidconsole-uclass.c

// SPDX-License-Identifier: GPL-2.0+
/*
 * Copyright (c) 2015 Google, Inc
 * (C) Copyright 2001-2015
 * DENX Software Engineering -- [email protected]
 * Compulab Ltd - http://compulab.co.il/
 * Bernecker & Rainer Industrieelektronik GmbH - http://www.br-automation.com
 */

#include <common.h>
#include <command.h>
#include <console.h>
#include <log.h>
#include <dm.h>
#include <video.h>
#include <video_console.h>
#include <video_font.h>		/* Bitmap font for code page 437 */
#include <linux/ctype.h>

/*
 * Structure to describe a console color
 */
struct vid_rgb {
	u32 r;
	u32 g;
	u32 b;
};

/* By default we scroll by a single line */
#ifndef CONFIG_CONSOLE_SCROLL_LINES
#define CONFIG_CONSOLE_SCROLL_LINES 1
#endif

int vidconsole_putc_xy(struct udevice *dev, uint x, uint y, char ch)
{
	struct vidconsole_ops *ops = vidconsole_get_ops(dev);

	if (!ops->putc_xy)
		return -ENOSYS;
	return ops->putc_xy(dev, x, y, ch);
}

int vidconsole_move_rows(struct udevice *dev, uint rowdst, uint rowsrc,
			 uint count)
{
	struct vidconsole_ops *ops = vidconsole_get_ops(dev);

	if (!ops->move_rows)
		return -ENOSYS;
	return ops->move_rows(dev, rowdst, rowsrc, count);
}

int vidconsole_set_row(struct udevice *dev, uint row, int clr)
{
	struct vidconsole_ops *ops = vidconsole_get_ops(dev);

	if (!ops->set_row)
		return -ENOSYS;
	return ops->set_row(dev, row, clr);
}

static int vidconsole_entry_start(struct udevice *dev)
{
	struct vidconsole_ops *ops = vidconsole_get_ops(dev);

	if (!ops->entry_start)
		return -ENOSYS;
	return ops->entry_start(dev);
}

/* Move backwards one space */
static int vidconsole_back(struct udevice *dev)
{
	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	struct vidconsole_ops *ops = vidconsole_get_ops(dev);
	int ret;

	if (ops->backspace) {
		ret = ops->backspace(dev);
		if (ret != -ENOSYS)
			return ret;
	}

	priv->xcur_frac -= VID_TO_POS(priv->x_charsize);
	if (priv->xcur_frac < priv->xstart_frac) {
		priv->xcur_frac = (priv->cols - 1) *
			VID_TO_POS(priv->x_charsize);
		priv->ycur -= priv->y_charsize;
		if (priv->ycur < 0)
			priv->ycur = 0;
	}
	video_sync(dev->parent, false);

	return 0;
}

/* Move to a newline, scrolling the display if necessary */
static void vidconsole_newline(struct udevice *dev)
{
	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	struct udevice *vid_dev = dev->parent;
	struct video_priv *vid_priv = dev_get_uclass_priv(vid_dev);
	const int rows = CONFIG_CONSOLE_SCROLL_LINES;
	int i;

	priv->xcur_frac = priv->xstart_frac;
	priv->ycur += priv->y_charsize;

	/* Check if we need to scroll the terminal */
	if ((priv->ycur + priv->y_charsize) / priv->y_charsize > priv->rows) {
		vidconsole_move_rows(dev, 0, rows, priv->rows - rows);
		for (i = 0; i < rows; i++)
			vidconsole_set_row(dev, priv->rows - i - 1,
					   vid_priv->colour_bg);
		priv->ycur -= rows * priv->y_charsize;
	}
	priv->last_ch = 0;

	video_sync(dev->parent, false);
}

static const struct vid_rgb colors[VID_COLOR_COUNT] = {
	{ 0x00, 0x00, 0x00 },  /* black */
	{ 0xc0, 0x00, 0x00 },  /* red */
	{ 0x00, 0xc0, 0x00 },  /* green */
	{ 0xc0, 0x60, 0x00 },  /* brown */
	{ 0x00, 0x00, 0xc0 },  /* blue */
	{ 0xc0, 0x00, 0xc0 },  /* magenta */
	{ 0x00, 0xc0, 0xc0 },  /* cyan */
	{ 0xc0, 0xc0, 0xc0 },  /* light gray */
	{ 0x80, 0x80, 0x80 },  /* gray */
	{ 0xff, 0x00, 0x00 },  /* bright red */
	{ 0x00, 0xff, 0x00 },  /* bright green */
	{ 0xff, 0xff, 0x00 },  /* yellow */
	{ 0x00, 0x00, 0xff },  /* bright blue */
	{ 0xff, 0x00, 0xff },  /* bright magenta */
	{ 0x00, 0xff, 0xff },  /* bright cyan */
	{ 0xff, 0xff, 0xff },  /* white */
};

u32 vid_console_color(struct video_priv *priv, unsigned int idx)
{
	switch (priv->bpix) {
	case VIDEO_BPP16:
		if (CONFIG_IS_ENABLED(VIDEO_BPP16)) {
			return ((colors[idx].r >> 3) << 11) |
			       ((colors[idx].g >> 2) <<  5) |
			       ((colors[idx].b >> 3) <<  0);
		}
		break;
	case VIDEO_BPP32:
		if (CONFIG_IS_ENABLED(VIDEO_BPP32)) {
			return (colors[idx].r << 16) |
			       (colors[idx].g <<  8) |
			       (colors[idx].b <<  0);
		}
		break;
	default:
		break;
	}

	/*
	 * For unknown bit arrangements just support
	 * black and white.
	 */
	if (idx)
		return 0xffffff; /* white */

	return 0x000000; /* black */
}

static char *parsenum(char *s, int *num)
{
	char *end;
	*num = simple_strtol(s, &end, 10);
	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
 * found, at which point the sequence is parsed and processed.
 */
static void vidconsole_escape_char(struct udevice *dev, char ch)
{
	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);

	if (!IS_ENABLED(CONFIG_VIDEO_ANSI))
		goto error;

	/* Sanity checking for bogus ESC sequences: */
	if (priv->escape_len >= sizeof(priv->escape_buf))
		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;

	/*
	 * Escape sequences are terminated by a letter, so keep
	 * accumulating until we get one:
	 */
	if (!isalpha(ch))
		return;

	/*
	 * clear escape mode first, otherwise things will get highly
	 * surprising if you hit any debug prints that come back to
	 * this console.
	 */
	priv->escape = 0;

	switch (ch) {
	case 'A':
	case 'B':
	case 'C':
	case 'D':
	case 'E':
	case 'F': {
		int row, col, num;
		char *s = priv->escape_buf;

		/*
		 * Cursor up/down: [%dA, [%dB, [%dE, [%dF
		 * Cursor left/right: [%dD, [%dC
		 */
		s++;    /* [ */
		s = parsenum(s, &num);
		if (num == 0)			/* No digit in sequence ... */
			num = 1;		/* ... means "move by 1". */

		get_cursor_position(priv, &row, &col);
		if (ch == 'A' || ch == 'F')
			row -= num;
		if (ch == 'C')
			col += num;
		if (ch == 'D')
			col -= num;
		if (ch == 'B' || ch == 'E')
			row += num;
		if (ch == 'E' || ch == 'F')
			col = 0;
		if (col < 0)
			col = 0;
		if (row < 0)
			row = 0;
		/* Right and bottom overflows are handled in the callee. */
		set_cursor_position(priv, row, col);
		break;
	}
	case 'H':
	case 'f': {
		int row, col;
		char *s = priv->escape_buf;

		/*
		 * Set cursor position: [%d;%df or [%d;%dH
		 */
		s++;    /* [ */
		s = parsenum(s, &row);
		s++;    /* ; */
		s = parsenum(s, &col);

		/*
		 * Video origin is [0, 0], terminal origin is [1, 1].
		 */
		if (row)
			--row;
		if (col)
			--col;

		set_cursor_position(priv, row, col);

		break;
	}
	case 'J': {
		int mode;

		/*
		 * Clear part/all screen:
		 *   [J or [0J - clear screen from cursor down
		 *   [1J       - clear screen from cursor up
		 *   [2J       - clear entire screen
		 *
		 * TODO we really only handle entire-screen case, others
		 * probably require some additions to video-uclass (and
		 * are not really needed yet by efi_console)
		 */
		parsenum(priv->escape_buf + 1, &mode);

		if (mode == 2) {
			video_clear(dev->parent);
			video_sync(dev->parent, false);
			priv->ycur = 0;
			priv->xcur_frac = priv->xstart_frac;
		} else {
			debug("unsupported clear mode: %d\n", mode);
		}
		break;
	}
	case 'K': {
		struct video_priv *vid_priv = dev_get_uclass_priv(dev->parent);
		int mode;

		/*
		 * Clear (parts of) current line
		 *   [0K       - clear line to end
		 *   [2K       - clear entire line
		 */
		parsenum(priv->escape_buf + 1, &mode);

		if (mode == 2) {
			int row, col;

			get_cursor_position(priv, &row, &col);
			vidconsole_set_row(dev, row, vid_priv->colour_bg);
		}
		break;
	}
	case 'm': {
		struct video_priv *vid_priv = dev_get_uclass_priv(dev->parent);
		char *s = priv->escape_buf;
		char *end = &priv->escape_buf[priv->escape_len];

		/*
		 * Set graphics mode: [%d;...;%dm
		 *
		 * Currently only supports the color attributes:
		 *
		 * Foreground Colors:
		 *
		 *   30	Black
		 *   31	Red
		 *   32	Green
		 *   33	Yellow
		 *   34	Blue
		 *   35	Magenta
		 *   36	Cyan
		 *   37	White
		 *
		 * Background Colors:
		 *
		 *   40	Black
		 *   41	Red
		 *   42	Green
		 *   43	Yellow
		 *   44	Blue
		 *   45	Magenta
		 *   46	Cyan
		 *   47	White
		 */

		s++;    /* [ */
		while (s < end) {
			int val;

			s = parsenum(s, &val);
			s++;

			switch (val) {
			case 0:
				/* all attributes off */
				video_set_default_colors(dev->parent, false);
				break;
			case 1:
				/* bold */
				vid_priv->fg_col_idx |= 8;
				vid_priv->colour_fg = vid_console_color(
						vid_priv, vid_priv->fg_col_idx);
				break;
			case 7:
				/* reverse video */
				vid_priv->colour_fg = vid_console_color(
						vid_priv, vid_priv->bg_col_idx);
				vid_priv->colour_bg = vid_console_color(
						vid_priv, vid_priv->fg_col_idx);
				break;
			case 30 ... 37:
				/* foreground color */
				vid_priv->fg_col_idx &= ~7;
				vid_priv->fg_col_idx |= val - 30;
				vid_priv->colour_fg = vid_console_color(
						vid_priv, vid_priv->fg_col_idx);
				break;
			case 40 ... 47:
				/* background color, also mask the bold bit */
				vid_priv->bg_col_idx &= ~0xf;
				vid_priv->bg_col_idx |= val - 40;
				vid_priv->colour_bg = vid_console_color(
						vid_priv, vid_priv->bg_col_idx);
				break;
			default:
				/* ignore unsupported SGR parameter */
				break;
			}
		}

		break;
	}
	default:
		debug("unrecognized escape sequence: %*s\n",
		      priv->escape_len, priv->escape_buf);
	}

	return;

error:
	/* something went wrong, just revert to normal mode: */
	priv->escape = 0;
}

/* Put that actual character on the screen (using the CP437 code page). */
static int vidconsole_output_glyph(struct udevice *dev, char ch)
{
	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	int ret;

	/*
	 * Failure of this function normally indicates an unsupported
	 * colour depth. Check this and return an error to help with
	 * diagnosis.
	 */
	ret = vidconsole_putc_xy(dev, priv->xcur_frac, priv->ycur, ch);
	if (ret == -EAGAIN) {
		vidconsole_newline(dev);
		ret = vidconsole_putc_xy(dev, priv->xcur_frac, priv->ycur, ch);
	}
	if (ret < 0)
		return ret;
	priv->xcur_frac += ret;
	priv->last_ch = ch;
	if (priv->xcur_frac >= priv->xsize_frac)
		vidconsole_newline(dev);

	return 0;
}

int vidconsole_put_char(struct udevice *dev, char ch)
{
	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	int ret;

	if (priv->escape) {
		vidconsole_escape_char(dev, ch);
		return 0;
	}

	switch (ch) {
	case '\x1b':
		priv->escape_len = 0;
		priv->escape = 1;
		break;
	case '\a':
		/* beep */
		break;
	case '\r':
		priv->xcur_frac = priv->xstart_frac;
		break;
	case '\n':
		vidconsole_newline(dev);
		vidconsole_entry_start(dev);
		break;
	case '\t':	/* Tab (8 chars alignment) */
		priv->xcur_frac = ((priv->xcur_frac / priv->tab_width_frac)
				+ 1) * priv->tab_width_frac;

		if (priv->xcur_frac >= priv->xsize_frac)
			vidconsole_newline(dev);
		break;
	case '\b':
		vidconsole_back(dev);
		priv->last_ch = 0;
		break;
	default:
		ret = vidconsole_output_glyph(dev, ch);
		if (ret < 0)
			return ret;
		break;
	}

	return 0;
}

int vidconsole_put_string(struct udevice *dev, const char *str)
{
	const char *s;
	int ret;

	for (s = str; *s; s++) {
		ret = vidconsole_put_char(dev, *s);
		if (ret)
			return ret;
	}

	return 0;
}

static void vidconsole_putc(struct stdio_dev *sdev, const char ch)
{
	struct udevice *dev = sdev->priv;
	int ret;

	ret = vidconsole_put_char(dev, ch);
	if (ret) {
#ifdef DEBUG
		console_puts_select_stderr(true, "[vc err: putc]");
#endif
	}
	video_sync(dev->parent, false);
}

static void vidconsole_puts(struct stdio_dev *sdev, const char *s)
{
	struct udevice *dev = sdev->priv;
	int ret;

	ret = vidconsole_put_string(dev, s);
	if (ret) {
#ifdef DEBUG
		char str[30];

		snprintf(str, sizeof(str), "[vc err: puts %d]", ret);
		console_puts_select_stderr(true, str);
#endif
	}
	video_sync(dev->parent, false);
}

/* Set up the number of rows and colours (rotated drivers override this) */
static int vidconsole_pre_probe(struct udevice *dev)
{
	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	struct udevice *vid = dev->parent;
	struct video_priv *vid_priv = dev_get_uclass_priv(vid);

	priv->xsize_frac = VID_TO_POS(vid_priv->xsize);

	return 0;
}

/* Register the device with stdio */
static int vidconsole_post_probe(struct udevice *dev)
{
	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	struct stdio_dev *sdev = &priv->sdev;

	if (!priv->tab_width_frac)
		priv->tab_width_frac = VID_TO_POS(priv->x_charsize) * 8;

	if (dev->seq) {
		snprintf(sdev->name, sizeof(sdev->name), "vidconsole%d",
			 dev->seq);
	} else {
		strcpy(sdev->name, "vidconsole");
	}

	sdev->flags = DEV_FLAGS_OUTPUT;
	sdev->putc = vidconsole_putc;
	sdev->puts = vidconsole_puts;
	sdev->priv = dev;

	return stdio_register(sdev);
}

UCLASS_DRIVER(vidconsole) = {
	.id		= UCLASS_VIDEO_CONSOLE,
	.name		= "vidconsole0",
	.pre_probe	= vidconsole_pre_probe,
	.post_probe	= vidconsole_post_probe,
	.per_device_auto_alloc_size	= sizeof(struct vidconsole_priv),
};

#ifdef CONFIG_VIDEO_COPY
int vidconsole_sync_copy(struct udevice *dev, void *from, void *to)
{
	struct udevice *vid = dev_get_parent(dev);

	return video_sync_copy(vid, from, to);
}

int vidconsole_memmove(struct udevice *dev, void *dst, const void *src,
		       int size)
{
	memmove(dst, src, size);
	return vidconsole_sync_copy(dev, dst, dst + size);
}
#endif

#if CONFIG_IS_ENABLED(CMD_VIDCONSOLE)
void vidconsole_position_cursor(struct udevice *dev, unsigned col, unsigned row)
{
	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	struct udevice *vid_dev = dev->parent;
	struct video_priv *vid_priv = dev_get_uclass_priv(vid_dev);

	col *= priv->x_charsize;
	row *= priv->y_charsize;
	priv->xcur_frac = VID_TO_POS(min_t(short, col, vid_priv->xsize - 1));
	priv->xstart_frac = priv->xcur_frac;
	priv->ycur = min_t(short, row, vid_priv->ysize - 1);
}

static int do_video_setcursor(struct cmd_tbl *cmdtp, int flag, int argc,
			      char *const argv[])
{
	unsigned int col, row;
	struct udevice *dev;

	if (argc != 3)
		return CMD_RET_USAGE;

	if (uclass_first_device_err(UCLASS_VIDEO_CONSOLE, &dev))
		return CMD_RET_FAILURE;
	col = simple_strtoul(argv[1], NULL, 10);
	row = simple_strtoul(argv[2], NULL, 10);
	vidconsole_position_cursor(dev, col, row);

	return 0;
}

static int do_video_puts(struct cmd_tbl *cmdtp, int flag, int argc,
			 char *const argv[])
{
	struct udevice *dev;
	const char *s;

	if (argc != 2)
		return CMD_RET_USAGE;

	if (uclass_first_device_err(UCLASS_VIDEO_CONSOLE, &dev))
		return CMD_RET_FAILURE;
	for (s = argv[1]; *s; s++)
		vidconsole_put_char(dev, *s);

	video_sync(dev->parent, false);

	return 0;
}

U_BOOT_CMD(
	setcurs, 3,	1,	do_video_setcursor,
	"set cursor position within screen",
	"    <col> <row> in character"
);

U_BOOT_CMD(
	lcdputs, 2,	1,	do_video_puts,
	"print string on video framebuffer",
	"    <string>"
);
#endif /* CONFIG_IS_ENABLED(CMD_VIDCONSOLE) */
Commit	Line	Data
83d290c5	1	// SPDX-License-Identifier: GPL-2.0+
83510766 SG	2	/*
	3	* Copyright (c) 2015 Google, Inc
	4	* (C) Copyright 2001-2015
	5	* DENX Software Engineering -- [email protected]
	6	* Compulab Ltd - http://compulab.co.il/
	7	* Bernecker & Rainer Industrieelektronik GmbH - http://www.br-automation.com
83510766 SG	8	*/
	9
	10	#include <common.h>
09140113	11	#include <command.h>
8b763dfd	12	#include <console.h>
f7ae49fc	13	#include <log.h>
83510766 SG	14	#include <dm.h>
	15	#include <video.h>
	16	#include <video_console.h>
5fba5329	17	#include <video_font.h> /* Bitmap font for code page 437 */
8b763dfd	18	#include <linux/ctype.h>
83510766	19
5c30fbb8 HS	20	/*
	21	* Structure to describe a console color
	22	*/
	23	struct vid_rgb {
	24	u32 r;
	25	u32 g;
	26	u32 b;
	27	};
	28
83510766 SG	29	/* By default we scroll by a single line */
	30	#ifndef CONFIG_CONSOLE_SCROLL_LINES
	31	#define CONFIG_CONSOLE_SCROLL_LINES 1
	32	#endif
	33
	34	int vidconsole_putc_xy(struct udevice *dev, uint x, uint y, char ch)
	35	{
	36	struct vidconsole_ops *ops = vidconsole_get_ops(dev);
	37
	38	if (!ops->putc_xy)
	39	return -ENOSYS;
	40	return ops->putc_xy(dev, x, y, ch);
	41	}
	42
	43	int vidconsole_move_rows(struct udevice *dev, uint rowdst, uint rowsrc,
	44	uint count)
	45	{
	46	struct vidconsole_ops *ops = vidconsole_get_ops(dev);
	47
	48	if (!ops->move_rows)
	49	return -ENOSYS;
	50	return ops->move_rows(dev, rowdst, rowsrc, count);
	51	}
	52
	53	int vidconsole_set_row(struct udevice *dev, uint row, int clr)
	54	{
	55	struct vidconsole_ops *ops = vidconsole_get_ops(dev);
	56
	57	if (!ops->set_row)
	58	return -ENOSYS;
	59	return ops->set_row(dev, row, clr);
	60	}
	61
58c733a7 SG	62	static int vidconsole_entry_start(struct udevice *dev)
	63	{
	64	struct vidconsole_ops *ops = vidconsole_get_ops(dev);
	65
	66	if (!ops->entry_start)
	67	return -ENOSYS;
	68	return ops->entry_start(dev);
	69	}
	70
83510766	71	/* Move backwards one space */
7b9f7e44	72	static int vidconsole_back(struct udevice *dev)
83510766 SG	73	{
83510766 SG	74	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
7b9f7e44 SG	75	struct vidconsole_ops *ops = vidconsole_get_ops(dev);
	76	int ret;
	77
	78	if (ops->backspace) {
	79	ret = ops->backspace(dev);
	80	if (ret != -ENOSYS)
	81	return ret;
	82	}
83510766	83
f2661786	84	priv->xcur_frac -= VID_TO_POS(priv->x_charsize);
c5b77d01	85	if (priv->xcur_frac < priv->xstart_frac) {
f2661786 SG	86	priv->xcur_frac = (priv->cols - 1) *
	87	VID_TO_POS(priv->x_charsize);
	88	priv->ycur -= priv->y_charsize;
	89	if (priv->ycur < 0)
	90	priv->ycur = 0;
83510766	91	}
55d39911	92	video_sync(dev->parent, false);
7b9f7e44 SG	93
7b9f7e44 SG	94	return 0;
83510766 SG	95	}
	96
	97	/* Move to a newline, scrolling the display if necessary */
	98	static void vidconsole_newline(struct udevice *dev)
	99	{
	100	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	101	struct udevice *vid_dev = dev->parent;
	102	struct video_priv *vid_priv = dev_get_uclass_priv(vid_dev);
	103	const int rows = CONFIG_CONSOLE_SCROLL_LINES;
	104	int i;
	105
c5b77d01	106	priv->xcur_frac = priv->xstart_frac;
f2661786	107	priv->ycur += priv->y_charsize;
83510766 SG	108
83510766 SG	109	/* Check if we need to scroll the terminal */
f2661786	110	if ((priv->ycur + priv->y_charsize) / priv->y_charsize > priv->rows) {
83510766 SG	111	vidconsole_move_rows(dev, 0, rows, priv->rows - rows);
	112	for (i = 0; i < rows; i++)
	113	vidconsole_set_row(dev, priv->rows - i - 1,
	114	vid_priv->colour_bg);
f2661786	115	priv->ycur -= rows * priv->y_charsize;
83510766	116	}
58c733a7 SG	117	priv->last_ch = 0;
58c733a7 SG	118
55d39911	119	video_sync(dev->parent, false);
83510766 SG	120	}
83510766 SG	121
5c30fbb8	122	static const struct vid_rgb colors[VID_COLOR_COUNT] = {
703d885c	123	{ 0x00, 0x00, 0x00 }, /* black */
9ffa4d12 HS	124	{ 0xc0, 0x00, 0x00 }, /* red */
	125	{ 0x00, 0xc0, 0x00 }, /* green */
	126	{ 0xc0, 0x60, 0x00 }, /* brown */
	127	{ 0x00, 0x00, 0xc0 }, /* blue */
	128	{ 0xc0, 0x00, 0xc0 }, /* magenta */
	129	{ 0x00, 0xc0, 0xc0 }, /* cyan */
	130	{ 0xc0, 0xc0, 0xc0 }, /* light gray */
	131	{ 0x80, 0x80, 0x80 }, /* gray */
	132	{ 0xff, 0x00, 0x00 }, /* bright red */
	133	{ 0x00, 0xff, 0x00 }, /* bright green */
703d885c	134	{ 0xff, 0xff, 0x00 }, /* yellow */
9ffa4d12 HS	135	{ 0x00, 0x00, 0xff }, /* bright blue */
	136	{ 0xff, 0x00, 0xff }, /* bright magenta */
	137	{ 0x00, 0xff, 0xff }, /* bright cyan */
703d885c RC	138	{ 0xff, 0xff, 0xff }, /* white */
	139	};
	140
5c30fbb8	141	u32 vid_console_color(struct video_priv *priv, unsigned int idx)
703d885c RC	142	{
	143	switch (priv->bpix) {
	144	case VIDEO_BPP16:
775d3322 SG	145	if (CONFIG_IS_ENABLED(VIDEO_BPP16)) {
	146	return ((colors[idx].r >> 3) << 11) \|
	147	((colors[idx].g >> 2) << 5) \|
	148	((colors[idx].b >> 3) << 0);
	149	}
ab6ea931	150	break;
703d885c	151	case VIDEO_BPP32:
775d3322 SG	152	if (CONFIG_IS_ENABLED(VIDEO_BPP32)) {
	153	return (colors[idx].r << 16) \|
	154	(colors[idx].g << 8) \|
	155	(colors[idx].b << 0);
	156	}
ab6ea931	157	break;
703d885c	158	default:
ab6ea931	159	break;
703d885c	160	}
ab6ea931 AG	161
	162	/*
	163	* For unknown bit arrangements just support
	164	* black and white.
	165	*/
	166	if (idx)
	167	return 0xffffff; /* white */
	168
	169	return 0x000000; /* black */
703d885c RC	170	}
703d885c RC	171
a085aa1f RC	172	static char parsenum(char s, int *num)
	173	{
	174	char *end;
	175	*num = simple_strtol(s, &end, 10);
	176	return end;
	177	}
	178
662f381a HS	179	/**
	180	* set_cursor_position() - set cursor position
	181	*
	182	* @priv: private data of the video console
	183	* @row: new row
	184	* @col: new column
	185	*/
	186	static void set_cursor_position(struct vidconsole_priv *priv, int row, int col)
	187	{
	188	/*
	189	* Ensure we stay in the bounds of the screen.
	190	*/
	191	if (row >= priv->rows)
	192	row = priv->rows - 1;
	193	if (col >= priv->cols)
	194	col = priv->cols - 1;
	195
	196	priv->ycur = row * priv->y_charsize;
	197	priv->xcur_frac = priv->xstart_frac +
	198	VID_TO_POS(col * priv->x_charsize);
	199	}
	200
	201	/**
	202	* get_cursor_position() - get cursor position
	203	*
	204	* @priv: private data of the video console
	205	* @row: row
	206	* @col: column
	207	*/
	208	static void get_cursor_position(struct vidconsole_priv *priv,
	209	int row, int col)
	210	{
	211	*row = priv->ycur / priv->y_charsize;
	212	*col = VID_TO_PIXEL(priv->xcur_frac - priv->xstart_frac) /
	213	priv->x_charsize;
	214	}
	215
a085aa1f RC	216	/*
	217	* Process a character while accumulating an escape string. Chars are
	218	* accumulated into escape_buf until the end of escape sequence is
	219	* found, at which point the sequence is parsed and processed.
	220	*/
	221	static void vidconsole_escape_char(struct udevice *dev, char ch)
	222	{
	223	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	224
	225	if (!IS_ENABLED(CONFIG_VIDEO_ANSI))
	226	goto error;
	227
	228	/* Sanity checking for bogus ESC sequences: */
	229	if (priv->escape_len >= sizeof(priv->escape_buf))
	230	goto error;
662f381a HS	231	if (priv->escape_len == 0) {
	232	switch (ch) {
	233	case '7':
	234	/* Save cursor position */
	235	get_cursor_position(priv, &priv->row_saved,
	236	&priv->col_saved);
	237	priv->escape = 0;
	238
	239	return;
	240	case '8': {
	241	/* Restore cursor position */
	242	int row = priv->row_saved;
	243	int col = priv->col_saved;
	244
	245	set_cursor_position(priv, row, col);
	246	priv->escape = 0;
	247	return;
	248	}
	249	case '[':
	250	break;
	251	default:
	252	goto error;
	253	}
	254	}
a085aa1f RC	255
	256	priv->escape_buf[priv->escape_len++] = ch;
	257
	258	/*
	259	* Escape sequences are terminated by a letter, so keep
	260	* accumulating until we get one:
	261	*/
	262	if (!isalpha(ch))
	263	return;
	264
	265	/*
	266	* clear escape mode first, otherwise things will get highly
	267	* surprising if you hit any debug prints that come back to
	268	* this console.
	269	*/
	270	priv->escape = 0;
	271
	272	switch (ch) {
29c158b9 AP	273	case 'A':
	274	case 'B':
	275	case 'C':
	276	case 'D':
	277	case 'E':
	278	case 'F': {
	279	int row, col, num;
	280	char *s = priv->escape_buf;
	281
	282	/*
	283	* Cursor up/down: [%dA, [%dB, [%dE, [%dF
	284	* Cursor left/right: [%dD, [%dC
	285	*/
	286	s++; /* [ */
	287	s = parsenum(s, &num);
	288	if (num == 0) /* No digit in sequence ... */
	289	num = 1; /* ... means "move by 1". */
	290
	291	get_cursor_position(priv, &row, &col);
	292	if (ch == 'A' \|\| ch == 'F')
	293	row -= num;
	294	if (ch == 'C')
	295	col += num;
	296	if (ch == 'D')
	297	col -= num;
	298	if (ch == 'B' \|\| ch == 'E')
	299	row += num;
	300	if (ch == 'E' \|\| ch == 'F')
	301	col = 0;
	302	if (col < 0)
	303	col = 0;
	304	if (row < 0)
	305	row = 0;
	306	/* Right and bottom overflows are handled in the callee. */
	307	set_cursor_position(priv, row, col);
	308	break;
	309	}
a085aa1f RC	310	case 'H':
	311	case 'f': {
	312	int row, col;
	313	char *s = priv->escape_buf;
	314
	315	/*
	316	* Set cursor position: [%d;%df or [%d;%dH
	317	*/
	318	s++; /* [ */
	319	s = parsenum(s, &row);
	320	s++; /* ; */
	321	s = parsenum(s, &col);
	322
118f020d HS	323	/*
	324	* Video origin is [0, 0], terminal origin is [1, 1].
	325	*/
	326	if (row)
	327	--row;
	328	if (col)
	329	--col;
	330
662f381a	331	set_cursor_position(priv, row, col);
a085aa1f RC	332
	333	break;
	334	}
	335	case 'J': {
	336	int mode;
	337
	338	/*
	339	* Clear part/all screen:
	340	* [J or [0J - clear screen from cursor down
	341	* [1J - clear screen from cursor up
	342	* [2J - clear entire screen
	343	*
	344	* TODO we really only handle entire-screen case, others
	345	* probably require some additions to video-uclass (and
	346	* are not really needed yet by efi_console)
	347	*/
	348	parsenum(priv->escape_buf + 1, &mode);
	349
	350	if (mode == 2) {
	351	video_clear(dev->parent);
55d39911	352	video_sync(dev->parent, false);
a085aa1f RC	353	priv->ycur = 0;
	354	priv->xcur_frac = priv->xstart_frac;
	355	} else {
	356	debug("unsupported clear mode: %d\n", mode);
	357	}
	358	break;
	359	}
4422294c AP	360	case 'K': {
	361	struct video_priv *vid_priv = dev_get_uclass_priv(dev->parent);
	362	int mode;
	363
	364	/*
	365	* Clear (parts of) current line
	366	* [0K - clear line to end
	367	* [2K - clear entire line
	368	*/
	369	parsenum(priv->escape_buf + 1, &mode);
	370
	371	if (mode == 2) {
	372	int row, col;
	373
	374	get_cursor_position(priv, &row, &col);
	375	vidconsole_set_row(dev, row, vid_priv->colour_bg);
	376	}
	377	break;
	378	}
703d885c RC	379	case 'm': {
	380	struct video_priv *vid_priv = dev_get_uclass_priv(dev->parent);
	381	char *s = priv->escape_buf;
	382	char *end = &priv->escape_buf[priv->escape_len];
	383
	384	/*
	385	* Set graphics mode: [%d;...;%dm
	386	*
	387	* Currently only supports the color attributes:
	388	*
	389	* Foreground Colors:
	390	*
	391	* 30 Black
	392	* 31 Red
	393	* 32 Green
	394	* 33 Yellow
	395	* 34 Blue
	396	* 35 Magenta
	397	* 36 Cyan
	398	* 37 White
	399	*
	400	* Background Colors:
	401	*
	402	* 40 Black
	403	* 41 Red
	404	* 42 Green
	405	* 43 Yellow
	406	* 44 Blue
	407	* 45 Magenta
	408	* 46 Cyan
	409	* 47 White
	410	*/
	411
	412	s++; /* [ */
	413	while (s < end) {
	414	int val;
	415
	416	s = parsenum(s, &val);
	417	s++;
	418
	419	switch (val) {
9ffa4d12 HS	420	case 0:
9ffa4d12 HS	421	/* all attributes off */
b9f210a3	422	video_set_default_colors(dev->parent, false);
9ffa4d12 HS	423	break;
	424	case 1:
	425	/* bold */
	426	vid_priv->fg_col_idx \|= 8;
	427	vid_priv->colour_fg = vid_console_color(
	428	vid_priv, vid_priv->fg_col_idx);
	429	break;
eabb0725 AP	430	case 7:
	431	/* reverse video */
	432	vid_priv->colour_fg = vid_console_color(
	433	vid_priv, vid_priv->bg_col_idx);
	434	vid_priv->colour_bg = vid_console_color(
	435	vid_priv, vid_priv->fg_col_idx);
	436	break;
703d885c	437	case 30 ... 37:
5c30fbb8	438	/* foreground color */
9ffa4d12 HS	439	vid_priv->fg_col_idx &= ~7;
9ffa4d12 HS	440	vid_priv->fg_col_idx \|= val - 30;
5c30fbb8	441	vid_priv->colour_fg = vid_console_color(
9ffa4d12	442	vid_priv, vid_priv->fg_col_idx);
703d885c RC	443	break;
703d885c RC	444	case 40 ... 47:
eabb0725 AP	445	/* background color, also mask the bold bit */
	446	vid_priv->bg_col_idx &= ~0xf;
	447	vid_priv->bg_col_idx \|= val - 40;
5c30fbb8	448	vid_priv->colour_bg = vid_console_color(
eabb0725	449	vid_priv, vid_priv->bg_col_idx);
703d885c RC	450	break;
703d885c RC	451	default:
5c30fbb8	452	/* ignore unsupported SGR parameter */
703d885c RC	453	break;
	454	}
	455	}
	456
	457	break;
	458	}
a085aa1f RC	459	default:
	460	debug("unrecognized escape sequence: %*s\n",
	461	priv->escape_len, priv->escape_buf);
	462	}
	463
	464	return;
	465
	466	error:
	467	/* something went wrong, just revert to normal mode: */
	468	priv->escape = 0;
	469	}
	470
7035ec3c AP	471	/* Put that actual character on the screen (using the CP437 code page). */
	472	static int vidconsole_output_glyph(struct udevice *dev, char ch)
	473	{
	474	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	475	int ret;
	476
	477	/*
	478	* Failure of this function normally indicates an unsupported
	479	* colour depth. Check this and return an error to help with
	480	* diagnosis.
	481	*/
	482	ret = vidconsole_putc_xy(dev, priv->xcur_frac, priv->ycur, ch);
	483	if (ret == -EAGAIN) {
	484	vidconsole_newline(dev);
	485	ret = vidconsole_putc_xy(dev, priv->xcur_frac, priv->ycur, ch);
	486	}
	487	if (ret < 0)
	488	return ret;
	489	priv->xcur_frac += ret;
	490	priv->last_ch = ch;
	491	if (priv->xcur_frac >= priv->xsize_frac)
	492	vidconsole_newline(dev);
	493
	494	return 0;
	495	}
	496
83510766 SG	497	int vidconsole_put_char(struct udevice *dev, char ch)
	498	{
	499	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	500	int ret;
	501
a085aa1f RC	502	if (priv->escape) {
	503	vidconsole_escape_char(dev, ch);
	504	return 0;
	505	}
	506
83510766	507	switch (ch) {
a085aa1f RC	508	case '\x1b':
	509	priv->escape_len = 0;
	510	priv->escape = 1;
	511	break;
5508f10a SG	512	case '\a':
	513	/* beep */
	514	break;
83510766	515	case '\r':
c5b77d01	516	priv->xcur_frac = priv->xstart_frac;
83510766 SG	517	break;
	518	case '\n':
	519	vidconsole_newline(dev);
58c733a7	520	vidconsole_entry_start(dev);
83510766 SG	521	break;
83510766 SG	522	case '\t': /* Tab (8 chars alignment) */
f2661786 SG	523	priv->xcur_frac = ((priv->xcur_frac / priv->tab_width_frac)
f2661786 SG	524	+ 1) * priv->tab_width_frac;
83510766	525
f2661786	526	if (priv->xcur_frac >= priv->xsize_frac)
83510766 SG	527	vidconsole_newline(dev);
	528	break;
	529	case '\b':
	530	vidconsole_back(dev);
58c733a7	531	priv->last_ch = 0;
83510766 SG	532	break;
83510766 SG	533	default:
7035ec3c	534	ret = vidconsole_output_glyph(dev, ch);
f2661786	535	if (ret < 0)
83510766	536	return ret;
83510766 SG	537	break;
	538	}
	539
	540	return 0;
	541	}
	542
e63168a9 MV	543	int vidconsole_put_string(struct udevice dev, const char str)
	544	{
	545	const char *s;
	546	int ret;
	547
	548	for (s = str; *s; s++) {
	549	ret = vidconsole_put_char(dev, *s);
	550	if (ret)
	551	return ret;
	552	}
	553
	554	return 0;
	555	}
	556
83510766 SG	557	static void vidconsole_putc(struct stdio_dev *sdev, const char ch)
	558	{
	559	struct udevice *dev = sdev->priv;
8b763dfd	560	int ret;
83510766	561
8b763dfd SG	562	ret = vidconsole_put_char(dev, ch);
	563	if (ret) {
	564	#ifdef DEBUG
	565	console_puts_select_stderr(true, "[vc err: putc]");
	566	#endif
	567	}
55d39911	568	video_sync(dev->parent, false);
83510766 SG	569	}
	570
	571	static void vidconsole_puts(struct stdio_dev sdev, const char s)
	572	{
	573	struct udevice *dev = sdev->priv;
8b763dfd SG	574	int ret;
	575
	576	ret = vidconsole_put_string(dev, s);
	577	if (ret) {
	578	#ifdef DEBUG
	579	char str[30];
83510766	580
8b763dfd SG	581	snprintf(str, sizeof(str), "[vc err: puts %d]", ret);
	582	console_puts_select_stderr(true, str);
	583	#endif
	584	}
55d39911	585	video_sync(dev->parent, false);
83510766 SG	586	}
	587
	588	/* Set up the number of rows and colours (rotated drivers override this) */
	589	static int vidconsole_pre_probe(struct udevice *dev)
	590	{
	591	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	592	struct udevice *vid = dev->parent;
	593	struct video_priv *vid_priv = dev_get_uclass_priv(vid);
	594
f2661786	595	priv->xsize_frac = VID_TO_POS(vid_priv->xsize);
83510766 SG	596
	597	return 0;
	598	}
	599
	600	/* Register the device with stdio */
	601	static int vidconsole_post_probe(struct udevice *dev)
	602	{
	603	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
	604	struct stdio_dev *sdev = &priv->sdev;
83510766	605
f2661786 SG	606	if (!priv->tab_width_frac)
	607	priv->tab_width_frac = VID_TO_POS(priv->x_charsize) * 8;
	608
f1a1247d SG	609	if (dev->seq) {
	610	snprintf(sdev->name, sizeof(sdev->name), "vidconsole%d",
	611	dev->seq);
	612	} else {
	613	strcpy(sdev->name, "vidconsole");
	614	}
f2661786	615
83510766 SG	616	sdev->flags = DEV_FLAGS_OUTPUT;
	617	sdev->putc = vidconsole_putc;
	618	sdev->puts = vidconsole_puts;
	619	sdev->priv = dev;
83510766	620
720873bf	621	return stdio_register(sdev);
83510766 SG	622	}
	623
	624	UCLASS_DRIVER(vidconsole) = {
	625	.id = UCLASS_VIDEO_CONSOLE,
	626	.name = "vidconsole0",
	627	.pre_probe = vidconsole_pre_probe,
	628	.post_probe = vidconsole_post_probe,
	629	.per_device_auto_alloc_size = sizeof(struct vidconsole_priv),
	630	};
	631
8c0b5d26 SG	632	#ifdef CONFIG_VIDEO_COPY
	633	int vidconsole_sync_copy(struct udevice dev, void from, void *to)
	634	{
	635	struct udevice *vid = dev_get_parent(dev);
	636
	637	return video_sync_copy(vid, from, to);
	638	}
	639
	640	int vidconsole_memmove(struct udevice dev, void dst, const void *src,
	641	int size)
	642	{
	643	memmove(dst, src, size);
	644	return vidconsole_sync_copy(dev, dst, dst + size);
	645	}
	646	#endif
	647
39b95556	648	#if CONFIG_IS_ENABLED(CMD_VIDCONSOLE)
83510766 SG	649	void vidconsole_position_cursor(struct udevice *dev, unsigned col, unsigned row)
	650	{
	651	struct vidconsole_priv *priv = dev_get_uclass_priv(dev);
f2661786 SG	652	struct udevice *vid_dev = dev->parent;
f2661786 SG	653	struct video_priv *vid_priv = dev_get_uclass_priv(vid_dev);
83510766	654
9949ee87 SG	655	col *= priv->x_charsize;
9949ee87 SG	656	row *= priv->y_charsize;
f2661786	657	priv->xcur_frac = VID_TO_POS(min_t(short, col, vid_priv->xsize - 1));
8cee2006	658	priv->xstart_frac = priv->xcur_frac;
f2661786	659	priv->ycur = min_t(short, row, vid_priv->ysize - 1);
83510766 SG	660	}
83510766 SG	661
09140113	662	static int do_video_setcursor(struct cmd_tbl *cmdtp, int flag, int argc,
83510766 SG	663	char *const argv[])
	664	{
	665	unsigned int col, row;
	666	struct udevice *dev;
	667
	668	if (argc != 3)
	669	return CMD_RET_USAGE;
	670
3f603cbb	671	if (uclass_first_device_err(UCLASS_VIDEO_CONSOLE, &dev))
83510766 SG	672	return CMD_RET_FAILURE;
	673	col = simple_strtoul(argv[1], NULL, 10);
	674	row = simple_strtoul(argv[2], NULL, 10);
	675	vidconsole_position_cursor(dev, col, row);
	676
	677	return 0;
	678	}
	679
09140113	680	static int do_video_puts(struct cmd_tbl *cmdtp, int flag, int argc,
83510766 SG	681	char *const argv[])
	682	{
	683	struct udevice *dev;
	684	const char *s;
	685
	686	if (argc != 2)
	687	return CMD_RET_USAGE;
	688
3f603cbb	689	if (uclass_first_device_err(UCLASS_VIDEO_CONSOLE, &dev))
83510766 SG	690	return CMD_RET_FAILURE;
	691	for (s = argv[1]; *s; s++)
	692	vidconsole_put_char(dev, *s);
	693
55d39911	694	video_sync(dev->parent, false);
889808da	695
83510766 SG	696	return 0;
	697	}
	698
	699	U_BOOT_CMD(
	700	setcurs, 3, 1, do_video_setcursor,
	701	"set cursor position within screen",
	702	" <col> <row> in character"
	703	);
	704
	705	U_BOOT_CMD(
	706	lcdputs, 2, 1, do_video_puts,
	707	"print string on video framebuffer",
	708	" <string>"
	709	);
39b95556	710	#endif /* CONFIG_IS_ENABLED(CMD_VIDCONSOLE) */