Fix VESA color palette corruption:

- VBE 3.0 says palette format resets to 6-bit mode when video mode changes.
We simply set 8-bit mode when we switch modes if the adapter supports it.
- VBE 3.0 also says if the mode is not VGA compatible, we must use VBE
function to save/restore palette.  Otherwise, VGA function may be used.
Thus, reinstate the save/load palette functions only for non-VGA compatible
modes regardless of its palette format.
- Let vesa(4) set VESA modes even if vga(4) claims to support it.
- Reset default palette if VESA pixel mode is set initially.
- Fix more style nits.

3 files changed, 40 insertions, 59 deletions

diff --git a/sys/dev/fb/vesa.c b/sys/dev/fb/vesa.c
index 222130124404..c2521c5ad48a 100644
--- a/sys/dev/fb/vesa.c
+++ b/sys/dev/fb/vesa.c
@@ -174,10 +174,8 @@ static int vesa_bios_save_palette2(int start, int colors, u_char *r, u_char *g,
 				   u_char *b, int bits);
 static int vesa_bios_load_palette(int start, int colors, u_char *palette,
 				  int bits);
-#ifdef notyet
 static int vesa_bios_load_palette2(int start, int colors, u_char *r, u_char *g,
 				   u_char *b, int bits);
-#endif
 #define STATE_SIZE	0
 #define STATE_SAVE	1
 #define STATE_LOAD	2
@@ -415,9 +413,9 @@ vesa_bios_save_palette(int start, int colors, u_char *palette, int bits)
 
 	bits = 8 - bits;
 	for (i = 0; i < colors; ++i) {
-		palette[i*3]     = p[i*4 + 2] << bits;
-		palette[i*3 + 1] = p[i*4 + 1] << bits;
-		palette[i*3 + 2] = p[i*4] << bits;
+		palette[i * 3] = p[i * 4 + 2] << bits;
+		palette[i * 3 + 1] = p[i * 4 + 1] << bits;
+		palette[i * 3 + 2] = p[i * 4] << bits;
 	}
 	x86bios_free(p, colors * 4);
 
@@ -455,9 +453,9 @@ vesa_bios_save_palette2(int start, int colors, u_char *r, u_char *g, u_char *b,
 
 	bits = 8 - bits;
 	for (i = 0; i < colors; ++i) {
-		r[i] = p[i*4 + 2] << bits;
-		g[i] = p[i*4 + 1] << bits;
-		b[i] = p[i*4] << bits;
+		r[i] = p[i * 4 + 2] << bits;
+		g[i] = p[i * 4 + 1] << bits;
+		b[i] = p[i * 4] << bits;
 	}
 	x86bios_free(p, colors * 4);
 
@@ -487,10 +485,10 @@ vesa_bios_load_palette(int start, int colors, u_char *palette, int bits)
 
 	bits = 8 - bits;
 	for (i = 0; i < colors; ++i) {
-		p[i*4]	   = palette[i*3 + 2] >> bits;
-		p[i*4 + 1] = palette[i*3 + 1] >> bits;
-		p[i*4 + 2] = palette[i*3] >> bits;
-		p[i*4 + 3] = 0;
+		p[i * 4] = palette[i * 3 + 2] >> bits;
+		p[i * 4 + 1] = palette[i * 3 + 1] >> bits;
+		p[i * 4 + 2] = palette[i * 3] >> bits;
+		p[i * 4 + 3] = 0;
 	}
 	x86bios_intr(&regs, 0x10);
 	x86bios_free(p, colors * 4);
@@ -498,7 +496,6 @@ vesa_bios_load_palette(int start, int colors, u_char *palette, int bits)
 	return (regs.R_AX != 0x004f);
 }
 
-#ifdef notyet
 static int
 vesa_bios_load_palette2(int start, int colors, u_char *r, u_char *g, u_char *b,
 			int bits)
@@ -523,17 +520,16 @@ vesa_bios_load_palette2(int start, int colors, u_char *r, u_char *g, u_char *b,
 
 	bits = 8 - bits;
 	for (i = 0; i < colors; ++i) {
-		p[i*4]	   = b[i] >> bits;
-		p[i*4 + 1] = g[i] >> bits;
-		p[i*4 + 2] = r[i] >> bits;
-		p[i*4 + 3] = 0;
+		p[i * 4] = b[i] >> bits;
+		p[i * 4 + 1] = g[i] >> bits;
+		p[i * 4 + 2] = r[i] >> bits;
+		p[i * 4 + 3] = 0;
 	}
 	x86bios_intr(&regs, 0x10);
 	x86bios_free(p, colors * 4);
 
 	return (regs.R_AX != 0x004f);
 }
-#endif
 
 static ssize_t
 vesa_bios_state_buf_size(void)
@@ -702,6 +698,7 @@ vesa_translate_flags(u_int16_t vflags)
 		{ V_MODECOLOR, V_INFO_COLOR, 0 },
 		{ V_MODEGRAPHICS, V_INFO_GRAPHICS, 0 },
 		{ V_MODELFB, V_INFO_LINEAR, 0 },
+		{ V_MODENONVGA, V_INFO_NONVGA, 0 },
 	};
 	int flags;
 	int i;
@@ -1275,7 +1272,8 @@ vesa_set_mode(video_adapter_t *adp, int mode)
 	 * the new mode correctly.
 	 */
 	if (VESA_MODE(adp->va_mode)) {
-		if ((*prevvidsw->get_info)(adp, mode, &info) == 0) {
+		if (!VESA_MODE(mode) &&
+		    (*prevvidsw->get_info)(adp, mode, &info) == 0) {
 			int10_set_mode(adp->va_initial_bios_mode);
 			if (adp->va_info.vi_flags & V_INFO_LINEAR)
 				vesa_unmap_buffer(adp->va_buffer,
@@ -1288,7 +1286,7 @@ vesa_set_mode(video_adapter_t *adp, int mode)
 	}
 
 	/* we may not need to handle this mode after all... */
-	if ((*prevvidsw->set_mode)(adp, mode) == 0)
+	if (!VESA_MODE(mode) && (*prevvidsw->set_mode)(adp, mode) == 0)
 		return (0);
 
 	/* is the new mode supported? */
@@ -1306,6 +1304,9 @@ vesa_set_mode(video_adapter_t *adp, int mode)
 	if (vesa_bios_set_mode(mode | ((info.vi_flags & V_INFO_LINEAR) ? 0x4000 : 0)))
 		return (1);
 
+	if ((vesa_adp_info->v_flags & V_DAC8) != 0)
+		vesa_bios_set_dac(8);
+
 	if (adp->va_info.vi_flags & V_INFO_LINEAR)
 		vesa_unmap_buffer(adp->va_buffer,
 				  vesa_adp_info->v_memsize*64*1024);
@@ -1382,17 +1383,11 @@ static int
 vesa_save_palette(video_adapter_t *adp, u_char *palette)
 {
 	int bits;
-	int error;
 
-	if ((adp == vesa_adp) && (vesa_adp_info->v_flags & V_DAC8)
-	    && VESA_MODE(adp->va_mode)) {
-		bits = vesa_bios_get_dac();
-		error = vesa_bios_save_palette(0, 256, palette, bits);
-		if (error == 0)
-			return (0);
-		if (bits != 6)
-			return (error);
-	}
+	if ((adp == vesa_adp) &&
+	    (adp->va_info.vi_flags & V_INFO_NONVGA) != 0 &&
+	    (bits = vesa_bios_get_dac()) >= 6)
+		return (vesa_bios_save_palette(0, 256, palette, bits));
 
 	return ((*prevvidsw->save_palette)(adp, palette));
 }
@@ -1400,19 +1395,12 @@ vesa_save_palette(video_adapter_t *adp, u_char *palette)
 static int
 vesa_load_palette(video_adapter_t *adp, u_char *palette)
 {
-#ifdef notyet
 	int bits;
-	int error;
 
-	if ((adp == vesa_adp) && (vesa_adp_info->v_flags & V_DAC8) 
-	    && VESA_MODE(adp->va_mode) && ((bits = vesa_bios_set_dac(8)) > 6)) {
-		error = vesa_bios_load_palette(0, 256, palette, bits);
-		if (error == 0)
-			return (0);
-		if (vesa_bios_set_dac(6) != 6)
-			return (1);
-	}
-#endif /* notyet */
+	if ((adp == vesa_adp) &&
+	    (adp->va_info.vi_flags & V_INFO_NONVGA) != 0 &&
+	    (bits = vesa_bios_get_dac()) >= 6)
+		return (vesa_bios_load_palette(0, 256, palette, bits));
 
 	return ((*prevvidsw->load_palette)(adp, palette));
 }
@@ -1637,14 +1625,11 @@ get_palette(video_adapter_t *adp, int base, int count,
 		return (1);
 	if ((base + count) > 256)
 		return (1);
-	if (!(vesa_adp_info->v_flags & V_DAC8) || !VESA_MODE(adp->va_mode))
+	if ((adp->va_info.vi_flags & V_INFO_NONVGA) == 0 ||
+	    (bits = vesa_bios_get_dac()) < 6)
 		return (1);
 
-	bits = vesa_bios_get_dac();
-	if (bits <= 6)
-		return (1);
-
-	r = malloc(count*3, M_DEVBUF, M_WAITOK);
+	r = malloc(count * 3, M_DEVBUF, M_WAITOK);
 	g = r + count;
 	b = g + count;
 	error = vesa_bios_save_palette2(base, count, r, g, b, bits);
@@ -1659,7 +1644,6 @@ get_palette(video_adapter_t *adp, int base, int count,
 	}
 	free(r, M_DEVBUF);
 
-	/* if error && bits != 6 at this point, we are in trouble... XXX */
 	return (error);
 }
 
@@ -1667,8 +1651,6 @@ static int
 set_palette(video_adapter_t *adp, int base, int count,
 	    u_char *red, u_char *green, u_char *blue, u_char *trans)
 {
-	return (1);
-#ifdef notyet
 	u_char *r;
 	u_char *g;
 	u_char *b;
@@ -1677,11 +1659,11 @@ set_palette(video_adapter_t *adp, int base, int count,
 
 	if ((base < 0) || (base >= 256) || (base + count > 256))
 		return (1);
-	if (!(vesa_adp_info->v_flags & V_DAC8) || !VESA_MODE(adp->va_mode)
-		|| ((bits = vesa_bios_set_dac(8)) <= 6))
+	if ((adp->va_info.vi_flags & V_INFO_NONVGA) == 0 ||
+	    (bits = vesa_bios_get_dac()) < 6)
 		return (1);
 
-	r = malloc(count*3, M_DEVBUF, M_WAITOK);
+	r = malloc(count * 3, M_DEVBUF, M_WAITOK);
 	g = r + count;
 	b = g + count;
 	copyin(red, r, count);
@@ -1690,13 +1672,8 @@ set_palette(video_adapter_t *adp, int base, int count,
 
 	error = vesa_bios_load_palette2(base, count, r, g, b, bits);
 	free(r, M_DEVBUF);
-	if (error == 0)
-		return (0);
 
-	/* if the following call fails, we are in trouble... XXX */
-	vesa_bios_set_dac(6);
-	return (1);
-#endif /* notyet */
+	return (error);
 }
 
 static int
diff --git a/sys/dev/syscons/syscons.c b/sys/dev/syscons/syscons.c
index 9ea8a6b9fe63..836db55eb5cf 100644
--- a/sys/dev/syscons/syscons.c
+++ b/sys/dev/syscons/syscons.c
@@ -414,6 +414,9 @@ sc_attach_unit(int unit, int flags)
 #endif
 	    sc_set_graphics_mode(scp, NULL, vmode);
 	    sc_set_pixel_mode(scp, NULL, 0, 0, 16, 8);
+#ifndef SC_NO_PALETTE_LOADING
+	    vidd_save_palette(sc->adp, sc->palette);
+#endif
 	    sc->initial_mode = vmode;
 #ifdef DEV_SPLASH
 	    /* put up the splash again! */
diff --git a/sys/sys/fbio.h b/sys/sys/fbio.h
index c7183dfc38c0..415ad966911f 100644
--- a/sys/sys/fbio.h
+++ b/sys/sys/fbio.h
@@ -269,6 +269,7 @@ struct video_info {
 #define V_INFO_GRAPHICS	(1 << 1)
 #define V_INFO_LINEAR	(1 << 2)
 #define V_INFO_VESA	(1 << 3)
+#define	V_INFO_NONVGA	(1 << 4)
     int			vi_width;
     int			vi_height;
     int			vi_cwidth;