1 files changed, 76 insertions, 17 deletions

diff --git a/drivers/video/fbconsole.c b/drivers/video/fbconsole.c
index 693c21f547..b261f17048 100644
--- a/drivers/video/fbconsole.c
+++ b/drivers/video/fbconsole.c
@@ -11,6 +11,7 @@ enum state_t {
 	LIT,				/* Literal input */
 	ESC,				/* Start of escape sequence */
 	CSI,				/* Reading arguments in "CSI Pn ;...*/
+	CSI_CNT,
 };
 
 struct fbc_priv {
@@ -34,10 +35,12 @@ struct fbc_priv {
 
 #define ANSI_FLAG_INVERT	(1 << 0)
 #define ANSI_FLAG_BRIGHT	(1 << 1)
+#define HIDE_CURSOR		(1 << 2)
 	unsigned flags;
 
 	int csipos;
 	u8 csi[256];
+	unsigned char csi_cmd;
 
 	int active;
 	int in_console;
@@ -144,6 +147,12 @@ static void video_invertchar(struct fbc_priv *priv, int x, int y)
 			priv->font->width, priv->font->height);
 }
 
+static void show_cursor(struct fbc_priv *priv, int x, int y)
+{
+	if (!(priv->flags & HIDE_CURSOR))
+		video_invertchar(priv, x, y);
+}
+
 static void printchar(struct fbc_priv *priv, int c)
 {
 	video_invertchar(priv, priv->x, priv->y);
@@ -200,7 +209,7 @@ static void printchar(struct fbc_priv *priv, int c)
 		priv->y = priv->rows;
 	}
 
-	video_invertchar(priv, priv->x, priv->y);
+	show_cursor(priv, priv->x, priv->y);
 
 	return;
 }
@@ -258,17 +267,51 @@ static void fbc_parse_csi(struct fbc_priv *priv)
 	case 'm':
 		fbc_parse_colors(priv);
 		return;
+	case '?': /* vt100: show/hide cursor */
+		priv->csi_cmd = last;
+		priv->state = CSI_CNT;
+		return;
+	case 'h':
+		/* suffix for vt100 "[?25h" */
+		switch (priv->csi_cmd) {
+		case '?': /* cursor visible */
+			priv->csi_cmd = -1;
+			if (!(priv->flags & HIDE_CURSOR))
+				break;
+
+			priv->flags &= ~HIDE_CURSOR;
+			/* show cursor now */
+			show_cursor(priv, priv->x, priv->y);
+			break;
+		}
+		break;
+	case 'l':
+		/* suffix for vt100 "[?25l" */
+		switch (priv->csi_cmd) {
+		case '?': /* cursor invisible */
+			priv->csi_cmd = -1;
+
+			/* hide cursor now */
+			video_invertchar(priv, priv->x, priv->y);
+			priv->flags |= HIDE_CURSOR;
+
+			break;
+		}
+		break;
 	case 'J':
 		cls(priv);
-		video_invertchar(priv, priv->x, priv->y);
+		show_cursor(priv, priv->x, priv->y);
 		return;
 	case 'H':
-		video_invertchar(priv, priv->x, priv->y);
+		show_cursor(priv, priv->x, priv->y);
+
 		pos = simple_strtoul(priv->csi, &end, 10);
-		priv->y = pos ? pos - 1 : 0;
+		priv->y = clamp(pos - 1, 0, (int) priv->rows);
+
 		pos = simple_strtoul(end + 1, NULL, 10);
-		priv->x = pos ? pos - 1 : 0;
-		video_invertchar(priv, priv->x, priv->y);
+		priv->x = clamp(pos - 1, 0, (int) priv->cols);
+
+		show_cursor(priv, priv->x, priv->y);
 	case 'K':
 		pos = simple_strtoul(priv->csi, &end, 10);
 		video_invertchar(priv, priv->x, priv->y);
@@ -292,6 +335,7 @@ static void fbc_putc(struct console_device *cdev, char c)
 {
 	struct fbc_priv *priv = container_of(cdev,
 					struct fbc_priv, cdev);
+	struct fb_info *fb = priv->fb;
 
 	if (priv->in_console)
 		return;
@@ -340,11 +384,18 @@ static void fbc_putc(struct console_device *cdev, char c)
 			break;
 		default:
 			fbc_parse_csi(priv);
-			priv->state = LIT;
+			if (priv->state != CSI_CNT)
+				priv->state = LIT;
 		}
 		break;
+	case CSI_CNT:
+		priv->state = CSI;
+		break;
+
 	}
 	priv->in_console = 0;
+
+	fb_flush(fb);
 }
 
 static int setup_font(struct fbc_priv *priv)
@@ -365,21 +416,13 @@ static int setup_font(struct fbc_priv *priv)
 	return 0;
 }
 
-static int fbc_set_active(struct console_device *cdev, unsigned flags)
+static int fbc_open(struct console_device *cdev)
 {
 	struct fbc_priv *priv = container_of(cdev,
 					struct fbc_priv, cdev);
 	struct fb_info *fb = priv->fb;
 	int ret;
 
-	if (priv->active) {
-		fb_close(priv->sc);
-		priv->active = false;
-	}
-
-	if (!(flags & (CONSOLE_STDOUT | CONSOLE_STDERR)))
-		return 0;
-
 	ret = setup_font(priv);
 	if (ret)
 		return ret;
@@ -400,6 +443,21 @@ static int fbc_set_active(struct console_device *cdev, unsigned flags)
 	return 0;
 }
 
+static int fbc_close(struct console_device *cdev)
+{
+	struct fbc_priv *priv = container_of(cdev,
+					struct fbc_priv, cdev);
+
+	if (priv->active) {
+		fb_close(priv->sc);
+		priv->active = false;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
 static int set_font(struct param_d *p, void *vpriv)
 {
 	struct fbc_priv *priv = vpriv;
@@ -434,7 +492,8 @@ int register_fbconsole(struct fb_info *fb)
 	cdev->getc = fbc_getc;
 	cdev->devname = "fbconsole";
 	cdev->devid = DEVICE_ID_DYNAMIC;
-	cdev->set_active = fbc_set_active;
+	cdev->open = fbc_open;
+	cdev->close = fbc_close;
 
 	ret = console_register(cdev);
 	if (ret) {