MFC 262785 263183 264182 264999 265391 265392 265395 265397 265398 265402 265403

    265442 265546 265680 265681 265719 265862 265864 265867 265927 266010 266495
    266540 266835 266856 266861 266862 267007 267310.

265391
  Define a new method for probing vt(4) driver before attach it at early stage.
265392
  Create dataset for vt(4) drivers.
265395
  Set of updates to vt(4) core part.
  o Declare vt(4) drivers dataset.
  o Create single static structures for all early drivers.
  o Add vt(4) to be by default in the kernel consoles list.
  o Create one more sysinit point, to be able to initialize memory and lock
  	requirement of early drivers.
  o Implement early drivers select. (Only best available will be selected).
  o Fix one missed "return (0)" for VTYLOCK.
  o Improve locking for cases when one driver replace another.
  o Make driver replacement notification less debug-look-like.
  o Minor spell fixes.
265397
  Switch fb and efifb drivers to use names and new vt(4) driver probe method.
265398
  Add vt(4) driver name for ofwfb driver.
265402
  Revert r264997 and r265026. It is not required anymore.
265403
  Switch vga drivers to use names and new vt(4) driver probe method.
265442
  Implement KDMKTONE ioctl.
265546
  Fix possible divide by zero.
265680
  No need to assign fields required and checked on probe.
265681
  Fix scrollback.
265719
  Hide debug messages under VT_DEBUG.
265927
  Update terminal sizes in any case when new vt(4) driver arrive.
  (Plus remove one unused newline)
266010
  Remove extra newlines.
  No functional changes.
266495
  Fix tty locking.
  o Correct expected values for VT_LOCKSWITCH ioctl.
  o Check current window for locked state.
266540
  Proper fix of VT_LOCKSWITCH ioctl.
266835
  Remove driver as unused.
267007
  Fix case when vt(4) started w/o driver assigned.
  o Always init locks and cv ASAP.
  o Initialize driver-independent parts even if driver probing fail.
  o Allow to call vt_upgrade anytime, for later loaded drivers.
  o New window flag VWF_READY, to track if window already initialized.
  Other updates:
  o Pass vd as a cookie for kbd_allocate.
  o Do not blank window on driver replacement.

Sponsored by:	The FreeBSD Foundation

12 files changed, 676 insertions, 288 deletions

diff --git a/sys/dev/vt/hw/efifb/efifb.c b/sys/dev/vt/hw/efifb/efifb.c
new file mode 100644
index 0000000..e140bfe
--- /dev/null
+++ b/sys/dev/vt/hw/efifb/efifb.c
@@ -0,0 +1,166 @@
+/*-
+ * Copyright (c) 2014 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * This software was developed by Aleksandr Rybalko under sponsorship from the
+ * FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/kernel.h>
+#include <sys/fbio.h>
+#include <sys/linker.h>
+
+#include "opt_platform.h"
+
+#include <machine/efi.h>
+#include <machine/metadata.h>
+#include <machine/vm.h>
+#include <machine/vmparam.h>
+#include <vm/vm.h>
+#include <vm/pmap.h>
+#include <machine/pmap.h>
+
+#include <dev/vt/vt.h>
+#include <dev/vt/hw/fb/vt_fb.h>
+#include <dev/vt/colors/vt_termcolors.h>
+
+static vd_init_t vt_efifb_init;
+static vd_probe_t vt_efifb_probe;
+
+static struct vt_driver vt_efifb_driver = {
+	.vd_name = "efifb",
+	.vd_probe = vt_efifb_probe,
+	.vd_init = vt_efifb_init,
+	.vd_blank = vt_fb_blank,
+	.vd_bitbltchr = vt_fb_bitbltchr,
+	.vd_maskbitbltchr = vt_fb_maskbitbltchr,
+	/* Better than VGA, but still generic driver. */
+	.vd_priority = VD_PRIORITY_GENERIC + 1,
+};
+
+static struct fb_info local_info;
+VT_DRIVER_DECLARE(vt_efifb, vt_efifb_driver);
+
+static int
+vt_efifb_probe(struct vt_device *vd)
+{
+	int		disabled;
+	struct efi_fb	*efifb;
+	caddr_t		kmdp;
+
+	disabled = 0;
+	TUNABLE_INT_FETCH("hw.syscons.disable", &disabled);
+	if (disabled != 0)
+		return (CN_DEAD);
+
+	kmdp = preload_search_by_type("elf kernel");
+	if (kmdp == NULL)
+		kmdp = preload_search_by_type("elf64 kernel");
+	efifb = (struct efi_fb *)preload_search_info(kmdp,
+	    MODINFO_METADATA | MODINFOMD_EFI_FB);
+	if (efifb == NULL)
+		return (CN_DEAD);
+
+	return (CN_INTERNAL);
+}
+
+static int
+vt_efifb_init(struct vt_device *vd)
+{
+	int		depth, d, i, len;
+	struct fb_info	*info;
+	struct efi_fb	*efifb;
+	caddr_t		kmdp;
+
+	info = vd->vd_softc;
+	if (info == NULL)
+		info = vd->vd_softc = (void *)&local_info;
+
+	kmdp = preload_search_by_type("elf kernel");
+	if (kmdp == NULL)
+		kmdp = preload_search_by_type("elf64 kernel");
+        efifb = (struct efi_fb *)preload_search_info(kmdp,
+            MODINFO_METADATA | MODINFOMD_EFI_FB);
+	if (efifb == NULL)
+		return (CN_DEAD);
+
+	info->fb_height = efifb->fb_height;
+	info->fb_width = efifb->fb_width;
+
+	depth = fls(efifb->fb_mask_red);
+	d = fls(efifb->fb_mask_green);
+	depth = d > depth ? d : depth;
+	d = fls(efifb->fb_mask_blue);
+	depth = d > depth ? d : depth;
+	d = fls(efifb->fb_mask_reserved);
+	depth = d > depth ? d : depth;
+	info->fb_depth = depth;
+
+	info->fb_stride = efifb->fb_stride * (depth / 8);
+
+	vt_generate_vga_palette(info->fb_cmap, COLOR_FORMAT_RGB,
+	    efifb->fb_mask_red, ffs(efifb->fb_mask_red) - 1,
+	    efifb->fb_mask_green, ffs(efifb->fb_mask_green) - 1,
+	    efifb->fb_mask_blue, ffs(efifb->fb_mask_blue) - 1);
+
+	info->fb_size = info->fb_height * info->fb_stride;
+	info->fb_pbase = efifb->fb_addr;
+	/*
+	 * We could use pmap_mapdev here except that the kernel pmap
+	 * hasn't been created yet and hence any attempt to lock it will
+	 * fail.
+	 */
+	info->fb_vbase = PHYS_TO_DMAP(efifb->fb_addr);
+
+	/* blank full size */
+	len = info->fb_size / 4;
+	for (i = 0; i < len; i++) {
+		((uint32_t *)info->fb_vbase)[i] = 0;
+	}
+
+	/* Get pixel storage size. */
+	info->fb_bpp = info->fb_stride / info->fb_width * 8;
+
+	/*
+	 * Early FB driver work with static window buffer, so reduce to minimal
+	 * size, buffer or screen.
+	 */
+	info->fb_width = MIN(info->fb_width, VT_FB_DEFAULT_WIDTH);
+	info->fb_height = MIN(info->fb_height, VT_FB_DEFAULT_HEIGHT);
+
+	fb_probe(info);
+	vt_fb_init(vd);
+
+	/* Clear the screen. */
+	vt_fb_blank(vd, TC_BLACK);
+
+	return (CN_INTERNAL);
+}
diff --git a/sys/dev/vt/hw/fb/vt_early_fb.c b/sys/dev/vt/hw/fb/vt_early_fb.c
index 4a81f4f..64b2122 100644
--- a/sys/dev/vt/hw/fb/vt_early_fb.c
+++ b/sys/dev/vt/hw/fb/vt_early_fb.c
@@ -52,18 +52,19 @@ __FBSDID("$FreeBSD$");
 #include <dev/vt/colors/vt_termcolors.h>
 
 static vd_init_t vt_efb_init;
+static vd_probe_t vt_efb_probe;
 
 static struct vt_driver vt_fb_early_driver = {
+	.vd_name = "efb",
+	.vd_probe = vt_efb_probe,
 	.vd_init = vt_efb_init,
 	.vd_blank = vt_fb_blank,
 	.vd_bitbltchr = vt_fb_bitbltchr,
 	.vd_priority = VD_PRIORITY_GENERIC,
 };
 
-static struct fb_info info;
-VT_CONSDEV_DECLARE(vt_fb_early_driver,
-    MAX(80, PIXEL_WIDTH(VT_FB_DEFAULT_WIDTH)),
-    MAX(25, PIXEL_HEIGHT(VT_FB_DEFAULT_HEIGHT)), &info);
+static struct fb_info local_info;
+VT_DRIVER_DECLARE(vt_efb, vt_fb_early_driver);
 
 static void
 #ifdef	FDT
@@ -126,30 +127,62 @@ vt_efb_initialize(struct fb_info *info)
         }
 }
 
-static int
-vt_efb_init(struct vt_device *vd)
+static phandle_t
+vt_efb_get_fbnode()
 {
-	struct ofw_pci_register pciaddrs[8];
-	struct fb_info *info;
-	int i, len, n_pciaddrs;
 	phandle_t chosen, node;
 	ihandle_t stdout;
 	char type[64];
 
-	info = vd->vd_softc;
-
 	chosen = OF_finddevice("/chosen");
 	OF_getprop(chosen, "stdout", &stdout, sizeof(stdout));
 	node = OF_instance_to_package(stdout);
-	if (node == -1) {
-		/*
-		 * The "/chosen/stdout" does not exist try
-		 * using "screen" directly.
-		 */
-		node = OF_finddevice("screen");
+	if (node != -1) {
+		/* The "/chosen/stdout" present. */
+		OF_getprop(node, "device_type", type, sizeof(type));
+		/* Check if it has "display" type. */
+		if (strcmp(type, "display") == 0)
+			return (node);
 	}
-	OF_getprop(node, "device_type", type, sizeof(type));
-	if (strcmp(type, "display") != 0)
+	/* Try device with name "screen". */
+	node = OF_finddevice("screen");
+
+	return (node);
+}
+
+static int
+vt_efb_probe(struct vt_device *vd)
+{
+	phandle_t node;
+
+	node = vt_efb_get_fbnode();
+	if (node == -1)
+		return (CN_DEAD);
+
+	if ((OF_getproplen(node, "height") <= 0) ||
+	    (OF_getproplen(node, "width") <= 0) ||
+	    (OF_getproplen(node, "depth") <= 0) ||
+	    (OF_getproplen(node, "linebytes") <= 0))
+		return (CN_DEAD);
+
+	return (CN_INTERNAL);
+}
+
+static int
+vt_efb_init(struct vt_device *vd)
+{
+	struct ofw_pci_register pciaddrs[8];
+	struct fb_info *info;
+	int i, len, n_pciaddrs;
+	phandle_t node;
+
+	if (vd->vd_softc == NULL)
+		vd->vd_softc = (void *)&local_info;
+
+	info = vd->vd_softc;
+
+	node = vt_efb_get_fbnode();
+	if (node == -1)
 		return (CN_DEAD);
 
 #define	GET(name, var)							\
@@ -249,7 +282,6 @@ vt_efb_init(struct vt_device *vd)
 	#endif
         }
 
-
 	/* blank full size */
 	len = info->fb_size / 4;
 	for (i = 0; i < len; i++) {
@@ -259,13 +291,6 @@ vt_efb_init(struct vt_device *vd)
 	/* Get pixel storage size. */
 	info->fb_bpp = info->fb_stride / info->fb_width * 8;
 
-	/*
-	 * Early FB driver work with static window buffer 80x25, so reduce
-	 * size to 640x480.
-	 */
-	info->fb_width = VT_FB_DEFAULT_WIDTH;
-	info->fb_height = VT_FB_DEFAULT_HEIGHT;
-
 #ifdef	FDT
 	vt_efb_initialize(info, node);
 #else
@@ -274,6 +299,5 @@ vt_efb_init(struct vt_device *vd)
 	fb_probe(info);
 	vt_fb_init(vd);
 
-
 	return (CN_INTERNAL);
 }
diff --git a/sys/dev/vt/hw/fb/vt_fb.c b/sys/dev/vt/hw/fb/vt_fb.c
index 7163bdc..3ffba40 100644
--- a/sys/dev/vt/hw/fb/vt_fb.c
+++ b/sys/dev/vt/hw/fb/vt_fb.c
@@ -50,9 +50,11 @@ void vt_fb_drawrect(struct vt_device *vd, int x1, int y1, int x2, int y2,
 void vt_fb_setpixel(struct vt_device *vd, int x, int y, term_color_t color);
 
 static struct vt_driver vt_fb_driver = {
+	.vd_name = "fb",
 	.vd_init = vt_fb_init,
 	.vd_blank = vt_fb_blank,
 	.vd_bitbltchr = vt_fb_bitbltchr,
+	.vd_maskbitbltchr = vt_fb_maskbitbltchr,
 	.vd_drawrect = vt_fb_drawrect,
 	.vd_setpixel = vt_fb_setpixel,
 	.vd_postswitch = vt_fb_postswitch,
@@ -61,6 +63,8 @@ static struct vt_driver vt_fb_driver = {
 	.vd_fb_mmap = vt_fb_mmap,
 };
 
+VT_DRIVER_DECLARE(vt_fb, vt_fb_driver);
+
 static int
 vt_fb_ioctl(struct vt_device *vd, u_long cmd, caddr_t data, struct thread *td)
 {
@@ -189,6 +193,68 @@ vt_fb_bitbltchr(struct vt_device *vd, const uint8_t *src, const uint8_t *mask,
 	uint32_t fgc, bgc, cc, o;
 	int c, l, bpp;
 	u_long line;
+	uint8_t b;
+	const uint8_t *ch;
+
+	info = vd->vd_softc;
+	bpp = FBTYPE_GET_BYTESPP(info);
+	fgc = info->fb_cmap[fg];
+	bgc = info->fb_cmap[bg];
+	b = 0;
+	if (bpl == 0)
+		bpl = (width + 7) >> 3; /* Bytes per sorce line. */
+
+	/* Don't try to put off screen pixels */
+	if (((left + width) > info->fb_width) || ((top + height) >
+	    info->fb_height))
+		return;
+
+	line = (info->fb_stride * top) + (left * bpp);
+	for (l = 0; l < height; l++) {
+		ch = src;
+		for (c = 0; c < width; c++) {
+			if (c % 8 == 0)
+				b = *ch++;
+			else
+				b <<= 1;
+			o = line + (c * bpp);
+			cc = b & 0x80 ? fgc : bgc;
+
+			switch(bpp) {
+			case 1:
+				info->wr1(info, o, cc);
+				break;
+			case 2:
+				info->wr2(info, o, cc);
+				break;
+			case 3:
+				/* Packed mode, so unaligned. Byte access. */
+				info->wr1(info, o, (cc >> 16) & 0xff);
+				info->wr1(info, o + 1, (cc >> 8) & 0xff);
+				info->wr1(info, o + 2, cc & 0xff);
+				break;
+			case 4:
+				info->wr4(info, o, cc);
+				break;
+			default:
+				/* panic? */
+				break;
+			}
+		}
+		line += info->fb_stride;
+		src += bpl;
+	}
+}
+
+void
+vt_fb_maskbitbltchr(struct vt_device *vd, const uint8_t *src, const uint8_t *mask,
+    int bpl, vt_axis_t top, vt_axis_t left, unsigned int width,
+    unsigned int height, term_color_t fg, term_color_t bg)
+{
+	struct fb_info *info;
+	uint32_t fgc, bgc, cc, o;
+	int c, l, bpp;
+	u_long line;
 	uint8_t b, m;
 	const uint8_t *ch;
 
diff --git a/sys/dev/vt/hw/fb/vt_fb.h b/sys/dev/vt/hw/fb/vt_fb.h
index 9b3aa0d..10dd238 100644
--- a/sys/dev/vt/hw/fb/vt_fb.h
+++ b/sys/dev/vt/hw/fb/vt_fb.h
@@ -41,7 +41,7 @@ int fb_probe(struct fb_info *info);
 vd_init_t	vt_fb_init;
 vd_blank_t	vt_fb_blank;
 vd_bitbltchr_t	vt_fb_bitbltchr;
+vd_maskbitbltchr_t vt_fb_maskbitbltchr;
 vd_postswitch_t	vt_fb_postswitch;
 
-
 #endif /* _DEV_VT_HW_FB_VT_FB_H_ */
diff --git a/sys/dev/vt/hw/ofwfb/ofwfb.c b/sys/dev/vt/hw/ofwfb/ofwfb.c
index b5f8e59..d91feff 100644
--- a/sys/dev/vt/hw/ofwfb/ofwfb.c
+++ b/sys/dev/vt/hw/ofwfb/ofwfb.c
@@ -49,6 +49,10 @@ __FBSDID("$FreeBSD$");
 struct ofwfb_softc {
 	phandle_t	sc_node;
 
+	struct ofw_pci_register sc_pciaddrs[8];
+	int		sc_num_pciaddrs;
+
+
 	intptr_t	sc_addr;
 	int		sc_depth;
 	int		sc_stride;
@@ -58,21 +62,50 @@ struct ofwfb_softc {
 	uint32_t	sc_colormap[16];
 };
 
+static vd_probe_t	ofwfb_probe;
 static vd_init_t	ofwfb_init;
 static vd_blank_t	ofwfb_blank;
 static vd_bitbltchr_t	ofwfb_bitbltchr;
+static vd_fb_mmap_t	ofwfb_mmap;
 
 static const struct vt_driver vt_ofwfb_driver = {
+	.vd_name	= "ofwfb",
+	.vd_probe	= ofwfb_probe,
 	.vd_init	= ofwfb_init,
 	.vd_blank	= ofwfb_blank,
 	.vd_bitbltchr	= ofwfb_bitbltchr,
+	.vd_maskbitbltchr = ofwfb_bitbltchr,
+	.vd_fb_mmap	= ofwfb_mmap,
 	.vd_priority	= VD_PRIORITY_GENERIC+1,
 };
 
 static struct ofwfb_softc ofwfb_conssoftc;
-VT_CONSDEV_DECLARE(vt_ofwfb_driver, PIXEL_WIDTH(1920), PIXEL_HEIGHT(1200),
-    &ofwfb_conssoftc);
-/* XXX: hardcoded max size */
+VT_DRIVER_DECLARE(vt_ofwfb, vt_ofwfb_driver);
+
+static int
+ofwfb_probe(struct vt_device *vd)
+{
+	phandle_t chosen, node;
+	ihandle_t stdout;
+	char type[64];
+
+	chosen = OF_finddevice("/chosen");
+	OF_getprop(chosen, "stdout", &stdout, sizeof(stdout));
+	node = OF_instance_to_package(stdout);
+	if (node == -1) {
+		/*
+		 * The "/chosen/stdout" does not exist try
+		 * using "screen" directly.
+		 */
+		node = OF_finddevice("screen");
+	}
+	OF_getprop(node, "device_type", type, sizeof(type));
+	if (strcmp(type, "display") != 0)
+		return (CN_DEAD);
+
+	/* Looks OK... */
+	return (CN_INTERNAL);
+}
 
 static void
 ofwfb_blank(struct vt_device *vd, term_color_t color)
@@ -109,6 +142,10 @@ ofwfb_bitbltchr(struct vt_device *vd, const uint8_t *src, const uint8_t *mask,
 	uint32_t fgc, bgc;
 	int c;
 	uint8_t b, m;
+	union {
+		uint32_t l;
+		uint8_t	 c[4];
+	} ch1, ch2;
 
 	fgc = sc->sc_colormap[fg];
 	bgc = sc->sc_colormap[bg];
@@ -120,36 +157,70 @@ ofwfb_bitbltchr(struct vt_device *vd, const uint8_t *src, const uint8_t *mask,
 		return;
 
 	line = (sc->sc_stride * top) + left * sc->sc_depth/8;
-	for (; height > 0; height--) {
-		for (c = 0; c < width; c++) {
-			if (c % 8 == 0)
+	if (mask == NULL && sc->sc_depth == 8 && (width % 8 == 0)) {
+		for (; height > 0; height--) {
+			for (c = 0; c < width; c += 8) {
 				b = *src++;
-			else
-				b <<= 1;
-			if (mask != NULL) {
+
+				/*
+				 * Assume that there is more background than
+				 * foreground in characters and init accordingly
+				 */
+				ch1.l = ch2.l = (bg << 24) | (bg << 16) |
+				    (bg << 8) | bg;
+
+				/*
+				 * Calculate 2 x 4-chars at a time, and then
+				 * write these out.
+				 */
+				if (b & 0x80) ch1.c[0] = fg;
+				if (b & 0x40) ch1.c[1] = fg;
+				if (b & 0x20) ch1.c[2] = fg;
+				if (b & 0x10) ch1.c[3] = fg;
+
+				if (b & 0x08) ch2.c[0] = fg;
+				if (b & 0x04) ch2.c[1] = fg;
+				if (b & 0x02) ch2.c[2] = fg;
+				if (b & 0x01) ch2.c[3] = fg;
+
+				*(uint32_t *)(sc->sc_addr + line + c) = ch1.l;
+				*(uint32_t *)(sc->sc_addr + line + c + 4) =
+				    ch2.l;
+			}
+			line += sc->sc_stride;
+		}
+	} else {
+		for (; height > 0; height--) {
+			for (c = 0; c < width; c++) {
 				if (c % 8 == 0)
-					m = *mask++;
+					b = *src++;
 				else
-					m <<= 1;
-				/* Skip pixel write, if mask has no bit set. */
-				if ((m & 0x80) == 0)
-					continue;
-			}
-			switch(sc->sc_depth) {
-			case 8:
-				*(uint8_t *)(sc->sc_addr + line + c) =
-				    b & 0x80 ? fg : bg;
-				break;
-			case 32:
-				*(uint32_t *)(sc->sc_addr + line + 4*c) = 
-				    (b & 0x80) ? fgc : bgc;
-				break;
-			default:
-				/* panic? */
-				break;
+					b <<= 1;
+				if (mask != NULL) {
+					if (c % 8 == 0)
+						m = *mask++;
+					else
+						m <<= 1;
+					/* Skip pixel write, if mask not set. */
+					if ((m & 0x80) == 0)
+						continue;
+				}
+				switch(sc->sc_depth) {
+				case 8:
+					*(uint8_t *)(sc->sc_addr + line + c) =
+					    b & 0x80 ? fg : bg;
+					break;
+				case 32:
+					*(uint32_t *)(sc->sc_addr + line + 4*c)
+					    = (b & 0x80) ? fgc : bgc;
+					break;
+				default:
+					/* panic? */
+					break;
+				}
 			}
+			line += sc->sc_stride;
 		}
-		line += sc->sc_stride;
 	}
 }
 
@@ -217,14 +288,12 @@ ofwfb_initialize(struct vt_device *vd)
 static int
 ofwfb_init(struct vt_device *vd)
 {
-	struct ofwfb_softc *sc = vd->vd_softc;
+	struct ofwfb_softc *sc;
 	char type[64];
 	phandle_t chosen;
 	ihandle_t stdout;
 	phandle_t node;
 	uint32_t depth, height, width;
-	struct ofw_pci_register pciaddrs[8];
-	int n_pciaddrs;
 	uint32_t fb_phys;
 	int i, len;
 #ifdef __sparc64__
@@ -233,6 +302,9 @@ ofwfb_init(struct vt_device *vd)
 	int space;
 #endif
 
+	/* Initialize softc */
+	vd->vd_softc = sc = &ofwfb_conssoftc;
+
 	chosen = OF_finddevice("/chosen");
 	OF_getprop(chosen, "stdout", &stdout, sizeof(stdout));
 	node = OF_instance_to_package(stdout);
@@ -275,15 +347,15 @@ ofwfb_init(struct vt_device *vd)
 	 * child of the PCI device: in that case, try the parent for
 	 * the assigned-addresses property.
 	 */
-	len = OF_getprop(node, "assigned-addresses", pciaddrs,
-	    sizeof(pciaddrs));
+	len = OF_getprop(node, "assigned-addresses", sc->sc_pciaddrs,
+	    sizeof(sc->sc_pciaddrs));
 	if (len == -1) {
 		len = OF_getprop(OF_parent(node), "assigned-addresses",
-		    pciaddrs, sizeof(pciaddrs));
+		    sc->sc_pciaddrs, sizeof(sc->sc_pciaddrs));
         }
         if (len == -1)
                 len = 0;
-	n_pciaddrs = len / sizeof(struct ofw_pci_register);
+	sc->sc_num_pciaddrs = len / sizeof(struct ofw_pci_register);
 
 	/*
 	 * Grab the physical address of the framebuffer, and then map it
@@ -313,13 +385,13 @@ ofwfb_init(struct vt_device *vd)
 		 * Linux does the same thing.
 		 */
 
-		fb_phys = n_pciaddrs;
-		for (i = 0; i < n_pciaddrs; i++) {
+		fb_phys = sc->sc_num_pciaddrs;
+		for (i = 0; i < sc->sc_num_pciaddrs; i++) {
 			/* If it is too small, not the framebuffer */
-			if (pciaddrs[i].size_lo < sc->sc_stride*height)
+			if (sc->sc_pciaddrs[i].size_lo < sc->sc_stride*height)
 				continue;
 			/* If it is not memory, it isn't either */
-			if (!(pciaddrs[i].phys_hi &
+			if (!(sc->sc_pciaddrs[i].phys_hi &
 			    OFW_PCI_PHYS_HI_SPACE_MEM32))
 				continue;
 
@@ -327,11 +399,12 @@ ofwfb_init(struct vt_device *vd)
 			fb_phys = i;
 
 			/* If it is prefetchable, it certainly is */
-			if (pciaddrs[i].phys_hi & OFW_PCI_PHYS_HI_PREFETCHABLE)
+			if (sc->sc_pciaddrs[i].phys_hi &
+			    OFW_PCI_PHYS_HI_PREFETCHABLE)
 				break;
 		}
 
-		if (fb_phys == n_pciaddrs) /* No candidates found */
+		if (fb_phys == sc->sc_num_pciaddrs) /* No candidates found */
 			return (CN_DEAD);
 
 	#if defined(__powerpc__)
@@ -348,3 +421,37 @@ ofwfb_init(struct vt_device *vd)
 	return (CN_INTERNAL);
 }
 
+static int
+ofwfb_mmap(struct vt_device *vd, vm_ooffset_t offset, vm_paddr_t *paddr,
+    int prot, vm_memattr_t *memattr)
+{
+	struct ofwfb_softc *sc = vd->vd_softc;
+        int i;
+
+	/*
+	 * Make sure the requested address lies within the PCI device's
+	 * assigned addrs
+	 */
+	for (i = 0; i < sc->sc_num_pciaddrs; i++)
+	  if (offset >= sc->sc_pciaddrs[i].phys_lo &&
+	    offset < (sc->sc_pciaddrs[i].phys_lo + sc->sc_pciaddrs[i].size_lo))
+		{
+			/*
+			 * If this is a prefetchable BAR, we can (and should)
+			 * enable write-combining.
+			 */
+			if (sc->sc_pciaddrs[i].phys_hi &
+			    OFW_PCI_PHYS_HI_PREFETCHABLE)
+				*memattr = VM_MEMATTR_WRITE_COMBINING;
+
+			*paddr = offset;
+			return (0);
+		}
+
+        /*
+         * Hack for Radeon...
+         */
+	*paddr = offset;
+	return (0);
+}
+
diff --git a/sys/dev/vt/hw/vga/vga.c b/sys/dev/vt/hw/vga/vga.c
index 13c640a..a13e094 100644
--- a/sys/dev/vt/hw/vga/vga.c
+++ b/sys/dev/vt/hw/vga/vga.c
@@ -71,6 +71,7 @@ struct vga_softc {
 #define	VT_VGA_HEIGHT	480
 #define	VT_VGA_MEMSIZE	(VT_VGA_WIDTH * VT_VGA_HEIGHT / 8)
 
+static vd_probe_t	vga_probe;
 static vd_init_t	vga_init;
 static vd_blank_t	vga_blank;
 static vd_bitbltchr_t	vga_bitbltchr;
@@ -81,6 +82,8 @@ static vd_putchar_t	vga_putchar;
 static vd_postswitch_t	vga_postswitch;
 
 static const struct vt_driver vt_vga_driver = {
+	.vd_name	= "vga",
+	.vd_probe	= vga_probe,
 	.vd_init	= vga_init,
 	.vd_blank	= vga_blank,
 	.vd_bitbltchr	= vga_bitbltchr,
@@ -97,8 +100,7 @@ static const struct vt_driver vt_vga_driver = {
  * buffer is always big enough to support both.
  */
 static struct vga_softc vga_conssoftc;
-VT_CONSDEV_DECLARE(vt_vga_driver, MAX(80, PIXEL_WIDTH(VT_VGA_WIDTH)),
-    MAX(25, PIXEL_HEIGHT(VT_VGA_HEIGHT)), &vga_conssoftc);
+VT_DRIVER_DECLARE(vt_vga, vt_vga_driver);
 
 static inline void
 vga_setcolor(struct vt_device *vd, term_color_t color)
@@ -349,7 +351,8 @@ static const struct unicp437 cp437table[] = {
 	{ 0x263a, 0x01, 0x01 }, { 0x263c, 0x0f, 0x00 },
 	{ 0x2640, 0x0c, 0x00 }, { 0x2642, 0x0b, 0x00 },
 	{ 0x2660, 0x06, 0x00 }, { 0x2663, 0x05, 0x00 },
-	{ 0x2665, 0x03, 0x01 }, { 0x266a, 0x0d, 0x01 },
+	{ 0x2665, 0x03, 0x01 }, { 0x266a, 0x0d, 0x00 },
+	{ 0x266c, 0x0e, 0x00 },
 };
 
 static uint8_t
@@ -631,10 +634,22 @@ vga_initialize(struct vt_device *vd, int textmode)
 }
 
 static int
+vga_probe(struct vt_device *vd)
+{
+
+	return (CN_INTERNAL);
+}
+
+static int
 vga_init(struct vt_device *vd)
 {
-	struct vga_softc *sc = vd->vd_softc;
-	int textmode = 0;
+	struct vga_softc *sc;
+	int textmode;
+
+	if (vd->vd_softc == NULL)
+		vd->vd_softc = (void *)&vga_conssoftc;
+	sc = vd->vd_softc;
+	textmode = 0;
 
 #if defined(__amd64__) || defined(__i386__)
 	sc->vga_fb_tag = X86_BUS_SPACE_MEM;
diff --git a/sys/dev/vt/hw/xboxfb/xboxfb.c b/sys/dev/vt/hw/xboxfb/xboxfb.c
deleted file mode 100644
index 8186f5e..0000000
--- a/sys/dev/vt/hw/xboxfb/xboxfb.c
+++ /dev/null
@@ -1,141 +0,0 @@
-/*-
- * Copyright (c) 2013 The FreeBSD Foundation
- * All rights reserved.
- *
- * This software was developed by Aleksandr Rybalko under sponsorship from the
- * FreeBSD Foundation.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- *
- * $FreeBSD$
- */
-
-#include <sys/cdefs.h>
-__FBSDID("$FreeBSD$");
-
-#include <sys/param.h>
-#include <sys/systm.h>
-#include <sys/kernel.h>
-#include <sys/fbio.h>
-
-#include "opt_platform.h"
-
-#include <dev/vt/vt.h>
-#include <dev/vt/hw/fb/vt_fb.h>
-#include <dev/vt/colors/vt_termcolors.h>
-
-#include <vm/vm.h>
-#include <vm/pmap.h>
-
-#include <machine/bus.h>
-#include <machine/pmap.h>
-#include <machine/vmparam.h>
-#include <machine/xbox.h>
-
-#define	VT_XBOX_WIDTH	640
-#define	VT_XBOX_HEIGHT	480
-
-static vd_init_t xboxfb_init;
-
-static struct vt_driver xboxfb_driver = {
-	.vd_init = xboxfb_init,
-	.vd_blank = vt_fb_blank,
-	.vd_bitbltchr = vt_fb_bitbltchr,
-	.vd_priority = VD_PRIORITY_GENERIC,
-};
-
-static struct fb_info xboxfb_info;
-VT_CONSDEV_DECLARE(xboxfb_driver, PIXEL_WIDTH(VT_XBOX_WIDTH),
-    PIXEL_HEIGHT(VT_XBOX_HEIGHT), &xboxfb_info);
-
-static int
-xboxfb_init(struct vt_device *vd)
-{
-	struct fb_info *info;
-	int i;
-
-	if (!arch_i386_is_xbox)
-		return (CN_DEAD);
-
-	info = &xboxfb_info;
-	/*
-	 * We must make a mapping from video framebuffer memory
-	 * to real. This is very crude:  we map the entire
-	 * videomemory to PAGE_SIZE! Since our kernel lives at
-	 * it's relocated address range (0xc0xxxxxx), it won't
-	 * care.
-	 *
-	 * We use address PAGE_SIZE and up so we can still trap
-	 * NULL pointers.  Once the real init is called, the
-	 * mapping will be done via the OS and stored in a more
-	 * sensible location ... but since we're not fully
-	 * initialized, this is our only way to go :-(
-	 */
-	for (i = 0; i < (XBOX_FB_SIZE / PAGE_SIZE); i++) {
-		pmap_kenter(((i + 1) * PAGE_SIZE),
-		    XBOX_FB_START + (i * PAGE_SIZE));
-	}
-	pmap_kenter((i + 1) * PAGE_SIZE,
-	    XBOX_FB_START_PTR - XBOX_FB_START_PTR % PAGE_SIZE);
-
-	/* Ensure the framebuffer is where we want it to be. */
-	*(uint32_t *)((i + 1) * PAGE_SIZE + XBOX_FB_START_PTR % PAGE_SIZE) =
-	    XBOX_FB_START;
-
-	/* Initialize fb_info. */
-	info = vd->vd_softc;
-
-	info->fb_width = VT_XBOX_WIDTH;
-	info->fb_height = VT_XBOX_HEIGHT;
-
-	info->fb_size = XBOX_FB_SIZE;
-	info->fb_stride = VT_XBOX_WIDTH * 4; /* 32bits per pixel. */
-
-	info->fb_vbase = PAGE_SIZE;
-	info->fb_pbase = XBOX_FB_START_PTR;
-
-	/* Get pixel storage size. */
-	info->fb_bpp = 32;
-	/* Get color depth. */
-	info->fb_depth = 24;
-
-	vt_generate_vga_palette(info->fb_cmap, COLOR_FORMAT_RGB, 255, 0, 255,
-	    8, 255, 16);
-	fb_probe(info);
-	vt_fb_init(vd);
-
-	return (CN_INTERNAL);
-}
-
-static void
-xbox_remap(void *unused)
-{
-	struct fb_info *info;
-
-	if (!arch_i386_is_xbox)
-		return;
-
-	info = &xboxfb_info;
-	info->fb_vbase = (intptr_t)pmap_mapdev(info->fb_pbase, info->fb_size);
-}
-
-SYSINIT(xboxfb, SI_SUB_DRIVERS, SI_ORDER_ANY, xbox_remap, NULL);
diff --git a/sys/dev/vt/vt.h b/sys/dev/vt/vt.h
index 9445cc9..87a57a2 100644
--- a/sys/dev/vt/vt.h
+++ b/sys/dev/vt/vt.h
@@ -78,7 +78,13 @@ one 'device sc' or 'device vt'"
 #endif /* defined(SC_TWOBUTTON_MOUSE) || defined(VT_TWOBUTTON_MOUSE) */
 
 #define	SC_DRIVER_NAME	"vt"
+#ifdef VT_DEBUG
 #define	DPRINTF(_l, ...)	if (vt_debug > (_l)) printf( __VA_ARGS__ )
+#define VT_CONSOLECTL_DEBUG
+#define VT_SYSMOUSE_DEBUG
+#else
+#define	DPRINTF(_l, ...)	do {} while (0)
+#endif
 #define	ISSIGVALID(sig)	((sig) > 0 && (sig) < NSIG)
 
 #define	VT_SYSCTL_INT(_name, _default, _descr)				\
@@ -253,6 +259,7 @@ struct vt_window {
 #define	VWF_CONSOLE	0x8	/* Kernel message console window. */
 #define	VWF_VTYLOCK	0x10	/* Prevent window switch. */
 #define	VWF_MOUSE_HIDE	0x20	/* Disable mouse events processing. */
+#define	VWF_READY	0x40	/* Window fully initialized. */
 #define	VWF_SWWAIT_REL	0x10000	/* Program wait for VT acquire is done. */
 #define	VWF_SWWAIT_ACQ	0x20000	/* Program wait for VT release is done. */
 	pid_t			 vw_pid;	/* Terminal holding process */
@@ -277,6 +284,7 @@ struct vt_window {
  */
 
 typedef int vd_init_t(struct vt_device *vd);
+typedef int vd_probe_t(struct vt_device *vd);
 typedef void vd_postswitch_t(struct vt_device *vd);
 typedef void vd_blank_t(struct vt_device *vd, term_color_t color);
 typedef void vd_bitbltchr_t(struct vt_device *vd, const uint8_t *src,
@@ -295,7 +303,9 @@ typedef void vd_drawrect_t(struct vt_device *, int, int, int, int, int,
 typedef void vd_setpixel_t(struct vt_device *, int, int, term_color_t);
 
 struct vt_driver {
+	char		 vd_name[16];
 	/* Console attachment. */
+	vd_probe_t	*vd_probe;
 	vd_init_t	*vd_init;
 
 	/* Drawing. */
@@ -337,10 +347,10 @@ void vt_upgrade(struct vt_device *vd);
 #define	PIXEL_HEIGHT(h)	((h) / 16)
 
 #ifndef VT_FB_DEFAULT_WIDTH
-#define	VT_FB_DEFAULT_WIDTH	640
+#define	VT_FB_DEFAULT_WIDTH	2048
 #endif
 #ifndef VT_FB_DEFAULT_HEIGHT
-#define	VT_FB_DEFAULT_HEIGHT	480
+#define	VT_FB_DEFAULT_HEIGHT	1200
 #endif
 
 #define	VT_CONSDEV_DECLARE(driver, width, height, softc)		\
@@ -391,6 +401,9 @@ TERMINAL_DECLARE_EARLY(driver ## _consterm, vt_termclass,		\
 SYSINIT(vt_early_cons, SI_SUB_INT_CONFIG_HOOKS, SI_ORDER_ANY,		\
     vt_upgrade, &driver ## _consdev)
 
+/* name argument is not used yet. */
+#define VT_DRIVER_DECLARE(name, drv) DATA_SET(vt_drv_set, drv)
+
 /*
  * Fonts.
  *
diff --git a/sys/dev/vt/vt_buf.c b/sys/dev/vt/vt_buf.c
index 0b078ec..d8602f8 100644
--- a/sys/dev/vt/vt_buf.c
+++ b/sys/dev/vt/vt_buf.c
@@ -357,17 +357,17 @@ void
 vtbuf_fill_locked(struct vt_buf *vb, const term_rect_t *r, term_char_t c)
 {
 	KASSERT(r->tr_begin.tp_row < vb->vb_scr_size.tp_row,
-	    ("vtbuf_fill_locked begin.tp_row %d must be < screen width %d",
+	    ("vtbuf_fill_locked begin.tp_row %d must be < screen height %d",
 		r->tr_begin.tp_row, vb->vb_scr_size.tp_row));
 	KASSERT(r->tr_begin.tp_col < vb->vb_scr_size.tp_col,
-	    ("vtbuf_fill_locked begin.tp_col %d must be < screen height %d",
+	    ("vtbuf_fill_locked begin.tp_col %d must be < screen width %d",
 		r->tr_begin.tp_col, vb->vb_scr_size.tp_col));
 
 	KASSERT(r->tr_end.tp_row <= vb->vb_scr_size.tp_row,
-	    ("vtbuf_fill_locked end.tp_row %d must be <= screen width %d",
+	    ("vtbuf_fill_locked end.tp_row %d must be <= screen height %d",
 		r->tr_end.tp_row, vb->vb_scr_size.tp_row));
 	KASSERT(r->tr_end.tp_col <= vb->vb_scr_size.tp_col,
-	    ("vtbuf_fill_locked end.tp_col %d must be <= screen height %d",
+	    ("vtbuf_fill_locked end.tp_col %d must be <= screen width %d",
 		r->tr_end.tp_col, vb->vb_scr_size.tp_col));
 
 	VTBUF_LOCK(vb);
@@ -448,8 +448,9 @@ vtbuf_grow(struct vt_buf *vb, const term_pos_t *p, int history_size)
 
 	history_size = MAX(history_size, p->tp_row);
 
-	if (history_size > vb->vb_history_size || p->tp_col >
-	    vb->vb_scr_size.tp_col) {
+	/* If new screen/history size bigger or buffer is VBF_STATIC. */
+	if ((history_size > vb->vb_history_size) || (p->tp_col >
+	    vb->vb_scr_size.tp_col) || (vb->vb_flags & VBF_STATIC)) {
 		/* Allocate new buffer. */
 		bufsize = history_size * p->tp_col * sizeof(term_char_t);
 		new = malloc(bufsize, M_VTBUF, M_WAITOK | M_ZERO);
@@ -495,6 +496,9 @@ vtbuf_grow(struct vt_buf *vb, const term_pos_t *p, int history_size)
 		/* Deallocate old buffer. */
 		free(old, M_VTBUF);
 		free(oldrows, M_VTBUF);
+	} else {
+		/* Just update the size. */
+		vb->vb_scr_size = *p;
 	}
 }
 
diff --git a/sys/dev/vt/vt_consolectl.c b/sys/dev/vt/vt_consolectl.c
index f9c4517..df8b341 100644
--- a/sys/dev/vt/vt_consolectl.c
+++ b/sys/dev/vt/vt_consolectl.c
@@ -61,8 +61,10 @@ consolectl_ioctl(struct cdev *dev, u_long cmd, caddr_t data, int flag,
 		return (0);
 	}
 	default:
+#ifdef VT_CONSOLECTL_DEBUG
 		printf("consolectl: unknown ioctl: %c:%lx\n",
 		    (char)IOCGROUP(cmd), IOCBASECMD(cmd));
+#endif
 		return (ENOIOCTL);
 	}
 }
diff --git a/sys/dev/vt/vt_core.c b/sys/dev/vt/vt_core.c
index eb2843e..80aa777 100644
--- a/sys/dev/vt/vt_core.c
+++ b/sys/dev/vt/vt_core.c
@@ -120,9 +120,10 @@ VT_SYSCTL_INT(debug, 0, "vt(9) debug level");
 VT_SYSCTL_INT(deadtimer, 15, "Time to wait busy process in VT_PROCESS mode");
 VT_SYSCTL_INT(suspendswitch, 1, "Switch to VT0 before suspend");
 
+static struct vt_device	vt_consdev;
 static unsigned int vt_unit = 0;
 static MALLOC_DEFINE(M_VT, "vt", "vt device");
-struct vt_device *main_vd = NULL;
+struct vt_device *main_vd = &vt_consdev;
 
 /* Boot logo. */
 extern unsigned int vt_logo_width;
@@ -144,6 +145,85 @@ static int vt_window_switch(struct vt_window *);
 static int vt_late_window_switch(struct vt_window *);
 static int vt_proc_alive(struct vt_window *);
 static void vt_resize(struct vt_device *);
+static void vt_update_static(void *);
+
+SET_DECLARE(vt_drv_set, struct vt_driver);
+
+#define _VTDEFH MAX(100, PIXEL_HEIGHT(VT_FB_DEFAULT_HEIGHT))
+#define _VTDEFW MAX(200, PIXEL_WIDTH(VT_FB_DEFAULT_WIDTH))
+
+static struct terminal	vt_consterm;
+static struct vt_window	vt_conswindow;
+static struct vt_device	vt_consdev = {
+	.vd_driver = NULL,
+	.vd_softc = NULL,
+	.vd_flags = VDF_INVALID,
+	.vd_windows = { [VT_CONSWINDOW] =  &vt_conswindow, },
+	.vd_curwindow = &vt_conswindow,
+	.vd_markedwin = NULL,
+	.vd_kbstate = 0,
+};
+static term_char_t vt_constextbuf[(_VTDEFW) * (VBF_DEFAULT_HISTORY_SIZE)];
+static term_char_t *vt_constextbufrows[VBF_DEFAULT_HISTORY_SIZE];
+static struct vt_window	vt_conswindow = {
+	.vw_number = VT_CONSWINDOW,
+	.vw_flags = VWF_CONSOLE,
+	.vw_buf = {
+		.vb_buffer = vt_constextbuf,
+		.vb_rows = vt_constextbufrows,
+		.vb_history_size = VBF_DEFAULT_HISTORY_SIZE,
+		.vb_curroffset = 0,
+		.vb_roffset = 0,
+		.vb_flags = VBF_STATIC,
+		.vb_mark_start = {.tp_row = 0, .tp_col = 0,},
+		.vb_mark_end = {.tp_row = 0, .tp_col = 0,},
+		.vb_scr_size = {
+			.tp_row = _VTDEFH,
+			.tp_col = _VTDEFW,
+		},
+	},
+	.vw_device = &vt_consdev,
+	.vw_terminal = &vt_consterm,
+	.vw_kbdmode = K_XLATE,
+};
+static struct terminal vt_consterm = {
+	.tm_class = &vt_termclass,
+	.tm_softc = &vt_conswindow,
+	.tm_flags = TF_CONS,
+};
+static struct consdev vt_consterm_consdev = {
+	.cn_ops = &termcn_cnops,
+	.cn_arg = &vt_consterm,
+	.cn_name = "ttyv0",
+};
+
+/* Add to set of consoles. */
+DATA_SET(cons_set, vt_consterm_consdev);
+
+/*
+ * Right after kmem is done to allow early drivers to use locking and allocate
+ * memory.
+ */
+SYSINIT(vt_update_static, SI_SUB_KMEM, SI_ORDER_ANY, vt_update_static,
+    &vt_consdev);
+/* Delay until all devices attached, to not waste time. */
+SYSINIT(vt_early_cons, SI_SUB_INT_CONFIG_HOOKS, SI_ORDER_ANY, vt_upgrade,
+    &vt_consdev);
+
+/* Initialize locks/mem depended members. */
+static void
+vt_update_static(void *dummy)
+{
+
+	if (main_vd->vd_driver != NULL)
+		printf("VT: running with driver \"%s\".\n",
+		    main_vd->vd_driver->vd_name);
+	else
+		printf("VT: init without driver.\n");
+
+	mtx_init(&main_vd->vd_lock, "vtdev", NULL, MTX_DEF);
+	cv_init(&main_vd->vd_winswitch, "vtwswt");
+}
 
 static void
 vt_switch_timer(void *arg)
@@ -203,12 +283,12 @@ vt_proc_window_switch(struct vt_window *vw)
 	struct vt_device *vd;
 	int ret;
 
-	if (vw->vw_flags & VWF_VTYLOCK)
-		return (EBUSY);
-
 	vd = vw->vw_device;
 	curvw = vd->vd_curwindow;
 
+	if (curvw->vw_flags & VWF_VTYLOCK)
+		return (EBUSY);
+
 	/* Ask current process permitions to switch away. */
 	if (curvw->vw_smode.mode == VT_PROCESS) {
 		DPRINTF(30, "%s: VT_PROCESS ", __func__);
@@ -556,11 +636,9 @@ vt_allocate_keyboard(struct vt_device *vd)
 	keyboard_t	*k0, *k;
 	keyboard_info_t	 ki;
 
-	idx0 = kbd_allocate("kbdmux", -1, (void *)&vd->vd_keyboard,
-	    vt_kbdevent, vd);
-	/* XXX: kb_token lost */
+	idx0 = kbd_allocate("kbdmux", -1, vd, vt_kbdevent, vd);
 	vd->vd_keyboard = idx0;
-	if (idx0 != -1) {
+	if (idx0 >= 0) {
 		DPRINTF(20, "%s: kbdmux allocated, idx = %d\n", __func__, idx0);
 		k0 = kbd_get_keyboard(idx0);
 
@@ -580,8 +658,11 @@ vt_allocate_keyboard(struct vt_device *vd)
 		}
 	} else {
 		DPRINTF(20, "%s: no kbdmux allocated\n", __func__);
-		idx0 = kbd_allocate("*", -1, (void *)&vd->vd_keyboard,
-		    vt_kbdevent, vd);
+		idx0 = kbd_allocate("*", -1, vd, vt_kbdevent, vd);
+		if (idx0 < 0) {
+			DPRINTF(10, "%s: No keyboard found.\n", __func__);
+			return (-1);
+		}
 	}
 	DPRINTF(20, "%s: vd_keyboard = %d\n", __func__, vd->vd_keyboard);
 
@@ -601,6 +682,22 @@ vtterm_bell(struct terminal *tm)
 }
 
 static void
+vtterm_beep(struct terminal *tm, u_int param)
+{
+	u_int freq, period;
+
+	if ((param == 0) || ((param & 0xffff) == 0)) {
+		vtterm_bell(tm);
+		return;
+	}
+
+	period = ((param >> 16) & 0xffff) * hz / 1000;
+	freq = 1193182 / (param & 0xffff);
+
+	sysbeep(freq, period);
+}
+
+static void
 vtterm_cursor(struct terminal *tm, const term_pos_t *p)
 {
 	struct vt_window *vw = tm->tm_softc;
@@ -775,7 +872,7 @@ vt_flush(struct vt_device *vd)
 	if ((vd->vd_flags & (VDF_MOUSECURSOR|VDF_TEXTMODE)) ==
 	    VDF_MOUSECURSOR) {
 		m = &vt_default_mouse_pointer;
-		bpl = (m->w + 7) >> 3; /* Bytes per sorce line. */
+		bpl = (m->w + 7) >> 3; /* Bytes per source line. */
 		w = m->w;
 		h = m->h;
 
@@ -851,9 +948,11 @@ vtterm_splash(struct vt_device *vd)
 }
 #endif
 
+
 static void
 vtterm_cnprobe(struct terminal *tm, struct consdev *cp)
 {
+	struct vt_driver *vtd, **vtdlist, *vtdbest = NULL;
 	struct vt_window *vw = tm->tm_softc;
 	struct vt_device *vd = vw->vw_device;
 	struct winsize wsz;
@@ -862,10 +961,27 @@ vtterm_cnprobe(struct terminal *tm, struct consdev *cp)
 		/* Initialization already done. */
 		return;
 
-	cp->cn_pri = vd->vd_driver->vd_init(vd);
+	SET_FOREACH(vtdlist, vt_drv_set) {
+		vtd = *vtdlist;
+		if (vtd->vd_probe == NULL)
+			continue;
+		if (vtd->vd_probe(vd) == CN_DEAD)
+			continue;
+		if ((vtdbest == NULL) ||
+		    (vtd->vd_priority > vtdbest->vd_priority))
+			vtdbest = vtd;
+	}
+	if (vtdbest == NULL) {
+		cp->cn_pri = CN_DEAD;
+		vd->vd_flags |= VDF_DEAD;
+	} else {
+		vd->vd_driver = vtdbest;
+		cp->cn_pri = vd->vd_driver->vd_init(vd);
+	}
+
+	/* Check if driver's vt_init return CN_DEAD. */
 	if (cp->cn_pri == CN_DEAD) {
 		vd->vd_flags |= VDF_DEAD;
-		return;
 	}
 
 	/* Initialize any early-boot keyboard drivers */
@@ -875,6 +991,7 @@ vtterm_cnprobe(struct terminal *tm, struct consdev *cp)
 	vd->vd_windows[VT_CONSWINDOW] = vw;
 	sprintf(cp->cn_name, "ttyv%r", VT_UNIT(vw));
 
+	/* Attach default font if not in TEXTMODE. */
 	if (!(vd->vd_flags & VDF_TEXTMODE))
 		vw->vw_font = vtfont_ref(&vt_font_default);
 
@@ -882,12 +999,12 @@ vtterm_cnprobe(struct terminal *tm, struct consdev *cp)
 	vt_winsize(vd, vw->vw_font, &wsz);
 	terminal_set_winsize(tm, &wsz);
 
+	if (vtdbest != NULL) {
 #ifdef DEV_SPLASH
-	vtterm_splash(vd);
+		vtterm_splash(vd);
 #endif
-
-	vd->vd_flags |= VDF_INITIALIZED;
-	main_vd = vd;
+		vd->vd_flags |= VDF_INITIALIZED;
+	}
 }
 
 static int
@@ -1058,22 +1175,27 @@ static int
 vt_set_border(struct vt_window *vw, struct vt_font *vf, term_color_t c)
 {
 	struct vt_device *vd = vw->vw_device;
-	int l, r, t, b, w, h;
+	int x, y, off_x, off_y;
 
 	if (vd->vd_driver->vd_drawrect == NULL)
 		return (ENOTSUP);
 
-	w = vd->vd_width - 1;
-	h = vd->vd_height - 1;
-	l = vd->vd_offset.tp_col - 1;
-	r = w - l;
-	t = vd->vd_offset.tp_row - 1;
-	b = h - t;
-
-	vd->vd_driver->vd_drawrect(vd, 0, 0, w, t, 1, c); /* Top bar. */
-	vd->vd_driver->vd_drawrect(vd, 0, t, l, b, 1, c); /* Left bar. */
-	vd->vd_driver->vd_drawrect(vd, r, t, w, b, 1, c); /* Right bar. */
-	vd->vd_driver->vd_drawrect(vd, 0, b, w, h, 1, c); /* Bottom bar. */
+	x = vd->vd_width - 1;
+	y = vd->vd_height - 1;
+	off_x = vd->vd_offset.tp_col;
+	off_y = vd->vd_offset.tp_row;
+
+	/* Top bar. */
+	if (off_y > 0)
+		vd->vd_driver->vd_drawrect(vd, 0, 0, x, off_y - 1, 1, c);
+	/* Left bar. */
+	if (off_x > 0)
+		vd->vd_driver->vd_drawrect(vd, 0, off_y, off_x - 1, y - off_y,
+		    1, c);
+	/* Right bar.  May be 1 pixel wider than necessary due to rounding. */
+	vd->vd_driver->vd_drawrect(vd, x - off_x, off_y, x, y - off_y, 1, c);
+	/* Bottom bar.  May be 1 mixel taller than necessary due to rounding. */
+	vd->vd_driver->vd_drawrect(vd, 0, y - off_y, x, y, 1, c);
 
 	return (0);
 }
@@ -1636,7 +1758,7 @@ skip_thunk:
 #endif
 		return (0);
 	case KDMKTONE:      	/* sound the bell */
-		/* TODO */
+		vtterm_beep(tm, *(u_int *)data);
 		return (0);
 	case KIOCSOUND:     	/* make tone (*data) hz */
 		/* TODO */
@@ -1701,10 +1823,13 @@ skip_thunk:
 		return (0);
 	case VT_LOCKSWITCH:
 		/* TODO: Check current state, switching can be in progress. */
-		if ((*(int *)data) & 0x01)
+		if ((*(int *)data) == 0x01)
 			vw->vw_flags |= VWF_VTYLOCK;
-		else
+		else if ((*(int *)data) == 0x02)
 			vw->vw_flags &= ~VWF_VTYLOCK;
+		else
+			return (EINVAL);
+		return (0);
 	case VT_OPENQRY:
 		VT_LOCK(vd);
 		for (i = 0; i < VT_MAXWINDOWS; i++) {
@@ -1866,43 +1991,45 @@ vt_upgrade(struct vt_device *vd)
 	struct vt_window *vw;
 	unsigned int i;
 
-	/* Device didn't pass vd_init() or already upgraded. */
-	if (vd->vd_flags & (VDF_ASYNC|VDF_DEAD))
-		return;
-	vd->vd_flags |= VDF_ASYNC;
-
-	mtx_init(&vd->vd_lock, "vtdev", NULL, MTX_DEF);
-	cv_init(&vd->vd_winswitch, "vtwswt");
-
-	/* Init 25 Hz timer. */
-	callout_init_mtx(&vd->vd_timer, &vd->vd_lock, 0);
-
 	for (i = 0; i < VT_MAXWINDOWS; i++) {
 		vw = vd->vd_windows[i];
 		if (vw == NULL) {
 			/* New window. */
 			vw = vt_allocate_window(vd, i);
-		} else if (vw->vw_flags & VWF_CONSOLE) {
-			/* For existing console window. */
-			callout_init(&vw->vw_proc_dead_timer, 0);
 		}
-		if (i == VT_CONSWINDOW) {
-			/* Console window. */
-			EVENTHANDLER_REGISTER(shutdown_pre_sync,
-			    vt_window_switch, vw, SHUTDOWN_PRI_DEFAULT);
+		if (!(vw->vw_flags & VWF_READY)) {
+			callout_init(&vw->vw_proc_dead_timer, 0);
+			terminal_maketty(vw->vw_terminal, "v%r", VT_UNIT(vw));
+			vw->vw_flags |= VWF_READY;
+			if (vw->vw_flags & VWF_CONSOLE) {
+				/* For existing console window. */
+				EVENTHANDLER_REGISTER(shutdown_pre_sync,
+				    vt_window_switch, vw, SHUTDOWN_PRI_DEFAULT);
+			}
 		}
-		terminal_maketty(vw->vw_terminal, "v%r", VT_UNIT(vw));
 
 	}
+	VT_LOCK(vd);
 	if (vd->vd_curwindow == NULL)
 		vd->vd_curwindow = vd->vd_windows[VT_CONSWINDOW];
 
+	if (!(vd->vd_flags & VDF_ASYNC)) {
 	/* Attach keyboard. */
 	vt_allocate_keyboard(vd);
 	DPRINTF(20, "%s: vd_keyboard = %d\n", __func__, vd->vd_keyboard);
 
-	/* Start timer when everything ready. */
-	callout_reset(&vd->vd_timer, hz / VT_TIMERFREQ, vt_timer, vd);
+		/* Init 25 Hz timer. */
+		callout_init_mtx(&vd->vd_timer, &vd->vd_lock, 0);
+
+		/* Start timer when everything ready. */
+		vd->vd_flags |= VDF_ASYNC;
+		callout_reset(&vd->vd_timer, hz / VT_TIMERFREQ, vt_timer, vd);
+	}
+
+	VT_UNLOCK(vd);
+
+	/* Refill settings with new sizes. */
+	vt_resize(vd);
 }
 
 static void
@@ -1913,9 +2040,11 @@ vt_resize(struct vt_device *vd)
 
 	for (i = 0; i < VT_MAXWINDOWS; i++) {
 		vw = vd->vd_windows[i];
+		VT_LOCK(vd);
 		/* Assign default font to window, if not textmode. */
 		if (!(vd->vd_flags & VDF_TEXTMODE) && vw->vw_font == NULL)
 			vw->vw_font = vtfont_ref(&vt_font_default);
+		VT_UNLOCK(vd);
 		/* Resize terminal windows */
 		vt_change_font(vw, vw->vw_font);
 	}
@@ -1927,23 +2056,25 @@ vt_allocate(struct vt_driver *drv, void *softc)
 	struct vt_device *vd;
 	struct winsize wsz;
 
-	if (main_vd == NULL) {
-		main_vd = malloc(sizeof *vd, M_VT, M_WAITOK|M_ZERO);
-		printf("%s: VT initialize with new VT driver.\n", __func__);
+	if (main_vd->vd_driver == NULL) {
+		main_vd->vd_driver = drv;
+		printf("VT: initialize with new VT driver \"%s\".\n",
+		    drv->vd_name);
 	} else {
 		/*
 		 * Check if have rights to replace current driver. For example:
 		 * it is bad idea to replace KMS driver with generic VGA one.
 		 */
 		if (drv->vd_priority <= main_vd->vd_driver->vd_priority) {
-			printf("%s: Driver priority %d too low. Current %d\n ",
-			    __func__, drv->vd_priority,
-			    main_vd->vd_driver->vd_priority);
+			printf("VT: Driver priority %d too low. Current %d\n ",
+			    drv->vd_priority, main_vd->vd_driver->vd_priority);
 			return;
 		}
-		printf("%s: Replace existing VT driver.\n", __func__);
+		printf("VT: Replacing driver \"%s\" with new \"%s\".\n",
+		    main_vd->vd_driver->vd_name, drv->vd_name);
 	}
 	vd = main_vd;
+	VT_LOCK(vd);
 	if (drv->vd_maskbitbltchr == NULL)
 		drv->vd_maskbitbltchr = drv->vd_bitbltchr;
 
@@ -1966,12 +2097,10 @@ vt_allocate(struct vt_driver *drv, void *softc)
 	vd->vd_driver = drv;
 	vd->vd_softc = softc;
 	vd->vd_driver->vd_init(vd);
+	VT_UNLOCK(vd);
 
 	vt_upgrade(vd);
 
-	/* Refill settings with new sizes. */
-	vt_resize(vd);
-
 #ifdef DEV_SPLASH
 	if (vd->vd_flags & VDF_SPLASH)
 		vtterm_splash(vd);
@@ -1986,7 +2115,8 @@ vt_allocate(struct vt_driver *drv, void *softc)
 
 	/* Update console window sizes to actual. */
 	vt_winsize(vd, vd->vd_windows[VT_CONSWINDOW]->vw_font, &wsz);
-	terminal_set_winsize(vd->vd_windows[VT_CONSWINDOW]->vw_terminal, &wsz);
+	terminal_set_winsize_blank(vd->vd_windows[VT_CONSWINDOW]->vw_terminal,
+	    &wsz, 0);
 }
 
 void
diff --git a/sys/dev/vt/vt_sysmouse.c b/sys/dev/vt/vt_sysmouse.c
index 73ef39d..92eee3c 100644
--- a/sys/dev/vt/vt_sysmouse.c
+++ b/sys/dev/vt/vt_sysmouse.c
@@ -376,8 +376,10 @@ sysmouse_ioctl(struct cdev *dev, u_long cmd, caddr_t data, int flag,
 	case MOUSE_MOUSECHAR:
 		return (0);
 	default:
+#ifdef VT_SYSMOUSE_DEBUG
 		printf("sysmouse: unknown ioctl: %c:%lx\n",
 		    (char)IOCGROUP(cmd), IOCBASECMD(cmd));
+#endif
 		return (ENOIOCTL);
 	}
 }