3 files changed, 83 insertions, 4 deletions

diff --git a/sys/dev/syscons/scterm-teken.c b/sys/dev/syscons/scterm-teken.c
index f24e52a04c29..64367624c44f 100644
--- a/sys/dev/syscons/scterm-teken.c
+++ b/sys/dev/syscons/scterm-teken.c
@@ -178,7 +178,7 @@ scteken_puts(scr_stat *scp, u_char *buf, int len, int kernel)
 	if (kernel) {
 		/* Use special colors for kernel messages. */
 		backup = *teken_get_curattr(&ts->ts_teken);
-		scteken_revattr(SC_KERNEL_CONS_ATTR, &kattr);
+		scteken_revattr(sc_kattr(), &kattr);
 		teken_set_curattr(&ts->ts_teken, &kattr);
 		teken_input(&ts->ts_teken, buf, len);
 		teken_set_curattr(&ts->ts_teken, &backup);
diff --git a/sys/dev/syscons/syscons.c b/sys/dev/syscons/syscons.c
index f93ed11b1547..91c882130c67 100644
--- a/sys/dev/syscons/syscons.c
+++ b/sys/dev/syscons/syscons.c
@@ -53,6 +53,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/lock.h>
 #include <sys/malloc.h>
 #include <sys/mutex.h>
+#include <sys/pcpu.h>
 #include <sys/priv.h>
 #include <sys/proc.h>
 #include <sys/random.h>
@@ -102,6 +103,8 @@ static default_attr user_default = {
     SC_NORM_REV_ATTR,
 };
 
+static	u_char		sc_kattrtab[MAXCPU];
+
 static	int		sc_console_unit = -1;
 static	int		sc_saver_keyb_only = 1;
 static  scr_stat    	*sc_console;
@@ -143,6 +146,8 @@ static	int		sc_no_suspend_vtswitch = 0;
 static	int		sc_susp_scr;
 
 static SYSCTL_NODE(_hw, OID_AUTO, syscons, CTLFLAG_RD, 0, "syscons");
+SYSCTL_OPAQUE(_hw_syscons, OID_AUTO, kattr, CTLFLAG_RW,
+    &sc_kattrtab, sizeof(sc_kattrtab), "CU", "kernel console attributes");
 static SYSCTL_NODE(_hw_syscons, OID_AUTO, saver, CTLFLAG_RD, 0, "saver");
 SYSCTL_INT(_hw_syscons_saver, OID_AUTO, keybonly, CTLFLAG_RW,
     &sc_saver_keyb_only, 0, "screen saver interrupted by input only");
@@ -263,6 +268,62 @@ static struct cdevsw consolectl_devsw = {
 	.d_name		= "consolectl",
 };
 
+/* ec -- emergency console. */
+
+static	u_int	ec_scroffset;
+
+static void
+ec_putc(int c)
+{
+	uintptr_t fb;
+	u_short *scrptr;
+	u_int ind;
+	int attr, column, mysize, width, xsize, yborder, ysize;
+
+	if (c < 0 || c > 0xff || c == '\a')
+		return;
+	if (sc_console == NULL) {
+#if !defined(__amd64__) && !defined(__i386__)
+		return;
+#endif
+		/*
+		 * This is enough for ec_putc() to work very early on x86
+		 * if the kernel starts in normal color text mode.
+		 */
+		fb = 0xb8000;
+		xsize = 80;
+		ysize = 25;
+	} else {
+		if (main_console.status & GRAPHICS_MODE)
+			return;
+		fb = main_console.sc->adp->va_window;
+		xsize = main_console.xsize;
+		ysize = main_console.ysize;
+	}
+	yborder = ysize / 5;
+	scrptr = (u_short *)(void *)fb + xsize * yborder;
+	mysize = xsize * (ysize - 2 * yborder);
+	do {
+		ind = ec_scroffset;
+		column = ind % xsize;
+		width = (c == '\b' ? -1 : c == '\t' ? (column + 8) & ~7 :
+		    c == '\r' ? -column : c == '\n' ? xsize - column : 1);
+		if (width == 0 || (width < 0 && ind < -width))
+			return;
+	} while (atomic_cmpset_rel_int(&ec_scroffset, ind, ind + width) == 0);
+	if (c == '\b' || c == '\r')
+		return;
+	if (c == '\n')
+		ind += xsize;	/* XXX clearing from new pos is not atomic */
+
+	attr = sc_kattr();
+	if (c == '\t' || c == '\n')
+		c = ' ';
+	do
+		scrptr[ind++ % mysize] = (attr << 8) | c;
+	while (--width != 0);
+}
+
 int
 sc_probe_unit(int unit, int flags)
 {
@@ -1859,10 +1920,13 @@ sc_cnputc(struct consdev *cd, int c)
     sc_cnputc_log[head % sizeof(sc_cnputc_log)] = c;
 
     /*
-     * If we couldn't open, return to defer output.
+     * If we couldn't open, do special reentrant output and return to defer
+     * normal output.
      */
-    if (!st.scr_opened)
+    if (!st.scr_opened) {
+	ec_putc(c);
 	return;
+    }
 
 #ifndef SC_NO_HISTORY
     if (scp == scp->sc->cur_scp && scp->status & SLKED) {
@@ -3009,8 +3073,16 @@ scinit(int unit, int flags)
     int i;
 
     /* one time initialization */
-    if (init_done == COLD)
+    if (init_done == COLD) {
 	sc_get_bios_values(&bios_value);
+	for (i = 0; i < nitems(sc_kattrtab); i++) {
+#if SC_KERNEL_CONS_ATTR == FG_WHITE
+	    sc_kattrtab[i] = 8 + (i + FG_WHITE) % 8U;
+#else
+	    sc_kattrtab[i] = SC_KERNEL_CONS_ATTR;
+#endif
+	}
+    }
     init_done = WARM;
 
     /*
@@ -4026,6 +4098,12 @@ sc_bell(scr_stat *scp, int pitch, int duration)
     }
 }
 
+int
+sc_kattr(void)
+{
+    return (sc_kattrtab[PCPU_GET(cpuid) % nitems(sc_kattrtab)]);
+}
+
 static void
 blink_screen(void *arg)
 {
diff --git a/sys/dev/syscons/syscons.h b/sys/dev/syscons/syscons.h
index c4f04ef40092..9fc616ee655e 100644
--- a/sys/dev/syscons/syscons.h
+++ b/sys/dev/syscons/syscons.h
@@ -591,6 +591,7 @@ void		sc_paste(scr_stat *scp, const u_char *p, int count);
 void		sc_respond(scr_stat *scp, const u_char *p,
 			   int count, int wakeup);
 void		sc_bell(scr_stat *scp, int pitch, int duration);
+int		sc_kattr(void);
 
 /* schistory.c */
 #ifndef SC_NO_HISTORY