 @@ -1,14 +1,14 @@
-/*	$NetBSD: machfb.c,v 1.97 2019/02/05 06:12:39 mrg Exp $	*/
+/*	$NetBSD: machfb.c,v 1.98 2019/12/07 01:00:40 macallan Exp $	*/
 /*
  * Copyright (c) 2002 Bang Jun-Young
  * Copyright (c) 2005, 2006, 2007 Michael Lorenz
  * All rights reserved.
+ *
  * 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
 @@ -24,27 +24,27 @@
  * 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.
  */
 /*
  * Some code is derived from ATI Rage Pro and Derivatives Programmer's Guide.
  */
 #include <sys/cdefs.h>
 __KERNEL_RCSID(0,
-	"$NetBSD: machfb.c,v 1.97 2019/02/05 06:12:39 mrg Exp $");
+	"$NetBSD: machfb.c,v 1.98 2019/12/07 01:00:40 macallan Exp $");
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/kernel.h>
 #include <sys/device.h>
 #include <sys/malloc.h>
 #include <sys/callout.h>
 #include <sys/lwp.h>
 #include <sys/kauth.h>
 #include <dev/videomode/videomode.h>
 #include <dev/videomode/edidvar.h>
 @@ -58,26 +58,32 @@ __KERNEL_RCSID(0,
 #include <dev/wscons/wsconsio.h>
 #include <dev/wsfont/wsfont.h>
 #include <dev/rasops/rasops.h>
 #include <dev/pci/wsdisplay_pci.h>
 #include <dev/wscons/wsdisplay_vconsvar.h>
 #include <dev/wscons/wsdisplay_glyphcachevar.h>
 #include "opt_wsemul.h"
 #include "opt_machfb.h"
 #include "opt_glyphcache.h"
 #ifdef MACHFB_DEBUG
 #define DPRINTF printf
 #else
 #define DPRINTF while (0) printf
 #endif
 #define MACH64_REG_SIZE		0x800
 #define MACH64_REG_OFF		0x7ff800
 #define	NBARS		3	/* number of Mach64 PCI BARs */
 struct vga_bar {
 	bus_addr_t vb_base;
 	bus_size_t vb_size;
 	pcireg_t vb_type;
 	int vb_flags;
 };
 struct mach64_softc {
 @@ -130,26 +136,27 @@ struct mach64_softc {
 	int ref_div;
 	int log2_vclk_post_div;
 	int vclk_post_div;
 	int vclk_fb_div;
 	int mclk_post_div;
 	int mclk_fb_div;
 	int sc_clock;	/* which clock to use */
 	int minref, m;
 	struct videomode *sc_my_mode;
 	int sc_edid_size;
 	uint8_t sc_edid_data[1024];
     	struct edid_info sc_ei;
     	int sc_setmode;
 	u_char sc_cmap_red[256];
 	u_char sc_cmap_green[256];
 	u_char sc_cmap_blue[256];
 	int sc_dacw, sc_blanked, sc_console;
 	struct vcons_data vd;
 	struct wsdisplay_accessops sc_accessops;
 	glyphcache sc_gc;
 };
 struct mach64_crtcregs {
 	uint32_t h_total_disp;
 	uint32_t h_sync_strt_wid;
 @@ -211,27 +218,29 @@ static const char *mach64_memtype_names[
 extern const u_char rasops_cmap[768];
 static int	mach64_match(device_t, cfdata_t, void *);
 static void	mach64_attach(device_t, device_t, void *);
 CFATTACH_DECL_NEW(machfb, sizeof(struct mach64_softc), mach64_match,
     mach64_attach, NULL, NULL);
 static void	mach64_init(struct mach64_softc *);
 static int	mach64_get_memsize(struct mach64_softc *);
 static int	mach64_get_max_ramdac(struct mach64_softc *);
 #if 0
 static void	mach64_get_mode(struct mach64_softc *, struct videomode *);
 #endif
 static int	mach64_calc_crtcregs(struct mach64_softc *,
 				     struct mach64_crtcregs *,
 				     struct videomode *);
 static void	mach64_set_crtcregs(struct mach64_softc *,
 				    struct mach64_crtcregs *);
 static int	mach64_modeswitch(struct mach64_softc *, struct videomode *);
 static void	mach64_set_dsp(struct mach64_softc *);
 static void	mach64_set_pll(struct mach64_softc *, int);
 static void	mach64_reset_engine(struct mach64_softc *);
 static void	mach64_init_engine(struct mach64_softc *);
 #if 0
 @@ -398,44 +407,45 @@ mach64_match(device_t parent, cfdata_t m
 static void
 mach64_attach(device_t parent, device_t self, void *aux)
+{
 	struct mach64_softc *sc = device_private(self);
 	struct pci_attach_args *pa = aux;
 	struct rasops_info *ri;
 	prop_data_t edid_data;
 	const struct videomode *mode = NULL;
 	int bar, id, expected_id;
 	int is_gx;
 	const char **memtype_names;
 	struct wsemuldisplaydev_attach_args aa;
 	long defattr;
-	int setmode = 0, width, height;
+	int width = 1024, height = 768;
 	pcireg_t screg;
 	uint32_t reg;
 	const pcireg_t enables = PCI_COMMAND_MEM_ENABLE;
 	int use_mmio = FALSE;
 	sc->sc_dev = self;
 	sc->sc_pc = pa->pa_pc;
 	sc->sc_pcitag = pa->pa_tag;
 	sc->sc_dacw = -1;
 	sc->sc_mode = WSDISPLAYIO_MODE_EMUL;
 	sc->sc_nbus = pa->pa_bus;
 	sc->sc_ndev = pa->pa_device;
 	sc->sc_nfunc = pa->pa_function;
 	sc->sc_locked = 0;
 	sc->sc_iot = pa->pa_iot;
 	sc->sc_accessops.ioctl = mach64_ioctl;
 	sc->sc_accessops.mmap = mach64_mmap;
 	sc->sc_setmode = 0;
 	pci_aprint_devinfo(pa, "Graphics processor");
 #ifdef MACHFB_DEBUG
 	printf(prop_dictionary_externalize(device_properties(self)));
 #endif
 	/* enable memory access */
 	screg = pci_conf_read(sc->sc_pc, sc->sc_pcitag, PCI_COMMAND_STATUS_REG);
 	if ((screg & enables) != enables) {
 		screg |= enables;
 		pci_conf_write(sc->sc_pc, sc->sc_pcitag,
 		    PCI_COMMAND_STATUS_REG, screg);
+	}
 @@ -488,42 +498,44 @@ mach64_attach(device_t parent, device_t
 	aprint_normal_dev(sc->sc_dev,
 	    "%d MB aperture at 0x%08x, %d KB registers at 0x%08x\n",
 	    (u_int)(sc->sc_apersize / (1024 * 1024)),
 	    (u_int)sc->sc_aperbase, (u_int)(sc->sc_regsize / 1024),
 	    (u_int)sc->sc_regbase);
 	printf("%s: %d KB ROM at 0x%08x\n", device_xname(sc->sc_dev),
 	    (int)sc->sc_rom.vb_size >> 10, (uint32_t)sc->sc_rom.vb_base);
 	prop_dictionary_get_uint32(device_properties(self), "width", &width);
 	prop_dictionary_get_uint32(device_properties(self), "height", &height);
 	default_mode.hdisplay = width;
 	default_mode.vdisplay = height;
 	memset(&sc->sc_ei, 0, sizeof(sc->sc_ei));
 	if ((edid_data = prop_dictionary_get(device_properties(self), "EDID"))
 	    != NULL) {
 		sc->sc_edid_size = uimin(1024, prop_data_size(edid_data));
 		memset(sc->sc_edid_data, 0, sizeof(sc->sc_edid_data));
 		memcpy(sc->sc_edid_data, prop_data_data_nocopy(edid_data),
 		    sc->sc_edid_size);
 		edid_parse(sc->sc_edid_data, &sc->sc_ei);
 #ifdef MACHFB_DEBUG
 		edid_print(&sc->sc_ei);
 #endif
+	}
 	is_gx = 0;
 	switch(mach64_chip_id) {
 		case PCI_PRODUCT_ATI_MACH64_GX:
 		case PCI_PRODUCT_ATI_MACH64_CX:
 			is_gx = 1;
 			/* FALLTHROUGH */
 		case PCI_PRODUCT_ATI_MACH64_CT:
 			sc->has_dsp = 0;
 			break;
 		case PCI_PRODUCT_ATI_MACH64_VT:
 		case PCI_PRODUCT_ATI_RAGE_II:
 			if((mach64_chip_rev & 0x07) == 0) {
 				sc->has_dsp = 0;
 @@ -551,38 +563,38 @@ mach64_attach(device_t parent, device_t
 	    ((mach64_chip_id >= PCI_PRODUCT_ATI_RAGE_LT_PRO_PCI) &&
 	    (mach64_chip_id <= PCI_PRODUCT_ATI_RAGE_LT_PRO))) {
 		aprint_normal_dev(sc->sc_dev, "ref_freq=29.498MHz\n");
 		sc->ref_freq = 29498;
 	} else
 		sc->ref_freq = 14318;
 	reg = regr(sc, CLOCK_CNTL);
 	aprint_debug("CLOCK_CNTL: %08x\n", reg);
 	sc->sc_clock = reg & 3;
 	aprint_debug("using clock %d\n", sc->sc_clock);
 	sc->ref_div = regrb_pll(sc, PLL_REF_DIV);
-	aprint_error("ref_div: %d\n", sc->ref_div);
+	DPRINTF("ref_div: %d\n", sc->ref_div);
 	sc->mclk_fb_div = regrb_pll(sc, MCLK_FB_DIV);
-	aprint_error("mclk_fb_div: %d\n", sc->mclk_fb_div);
+	DPRINTF("mclk_fb_div: %d\n", sc->mclk_fb_div);
 	sc->mem_freq = (2 * sc->ref_freq * sc->mclk_fb_div) /
 	    (sc->ref_div * 2);
 	sc->mclk_post_div = (sc->mclk_fb_div * 2 * sc->ref_freq) /
 	    (sc->mem_freq * sc->ref_div);
 	sc->ramdac_freq = mach64_get_max_ramdac(sc);
+	{
 		sc->minref = sc->ramdac_freq / 510;
 		sc->m = sc->ref_freq / sc->minref;
-		aprint_error("minref: %d m: %d\n", sc->minref, sc->m);
+		DPRINTF("minref: %d m: %d\n", sc->minref, sc->m);
+	}
 	aprint_normal_dev(sc->sc_dev,
 	    "%ld KB %s %d.%d MHz, maximum RAMDAC clock %d MHz\n",
 	    (u_long)sc->memsize,
 	    memtype_names[sc->memtype],
 	    sc->mem_freq / 1000, sc->mem_freq % 1000,
 	    sc->ramdac_freq / 1000);
 	id = regr(sc, CONFIG_CHIP_ID) & 0xffff;
 	switch(mach64_chip_id) {
 		case PCI_PRODUCT_ATI_MACH64_GX:
 			expected_id = 0x00d7;
 			break;
 @@ -594,102 +606,109 @@ mach64_attach(device_t parent, device_t
 			expected_id = mach64_chip_id;
+	}
 	if (id != expected_id) {
 		aprint_error_dev(sc->sc_dev,
 		    "chip ID mismatch, 0x%x != 0x%x\n", id, expected_id);
 		return;
+	}
 	sc->sc_console = mach64_is_console(sc);
 	aprint_debug("gen_cntl: %08x\n", regr(sc, CRTC_GEN_CNTL));
 #define MODE_IS_VALID(m) ((sc->ramdac_freq >= (m)->dot_clock) && \
-			  ((m)->hdisplay <= 11280))
+			  ((m)->hdisplay <= 1280))
 	/* no mode setting support on ancient chips with external clocks */
-	setmode = 0;
+	sc->sc_setmode = 0;
 	if (!is_gx) {
 		/*
 		 * Now pick a mode.
 		 */
 		if ((sc->sc_ei.edid_preferred_mode != NULL)) {
 			struct videomode *m = sc->sc_ei.edid_preferred_mode;
 			if (MODE_IS_VALID(m)) {
 				memcpy(&default_mode, m,
 				    sizeof(struct videomode));
-				setmode = 1;
+				sc->sc_setmode = 1;
 			} else {
 				aprint_error_dev(sc->sc_dev,
 				    "unable to use preferred mode\n");
+			}
+		}
 		/*
 		 * if we can't use the preferred mode go look for the
 		 * best one we can support
 		 */
-		if (setmode == 0) {
+		if (sc->sc_setmode == 0) {
 			struct videomode *m = sc->sc_ei.edid_modes;
 			mode = NULL;
 			sort_modes(sc->sc_ei.edid_modes,
 			    &sc->sc_ei.edid_preferred_mode,
 			    sc->sc_ei.edid_nmodes);
 			for (int n = 0; n < sc->sc_ei.edid_nmodes; n++)
 				if (MODE_IS_VALID(&m[n])) {
 					mode = &m[n];
 					break;
+				}
 			if (mode != NULL) {
 				memcpy(&default_mode, mode,
 				    sizeof(struct videomode));
-				setmode = 1;
+				sc->sc_setmode = 1;
+			}
+		}
 		/* got nothing? try to pick one based on firmware parameters */
 		if (setmode == 0 && width > 0 && height > 0) {
-			/* no EDID data? */
+	/* make sure my_mode points at something sensible if the above fails */
-			mode = pick_mode_by_ref(width, height, 60);
+	if (default_mode.dot_clock == 0) {
-			memcpy(&default_mode, mode, sizeof(struct videomode));
+		sc->sc_setmode = 0;
-			setmode = 1;
+		mode = pick_mode_by_ref(width, height, 60);
 		if (mode != NULL) {
-		/* still nothing? Grab the default */
+			memcpy(&default_mode, mode, sizeof(default_mode));
-		if (setmode == 0) {
+		} else if ((width > 0) && (height > 0)) {
 			default_mode.hdisplay = width;
 			default_mode.vdisplay = height;
 		} else {
 			/*
 			 * if we end up here we're probably dealing with
 			 * uninitialized hardware - try to set 1024x768@60 and
 			 * hope for the best...
 			 */
 			mode = pick_mode_by_ref(1024, 768, 60);
-			memcpy(&default_mode, mode, sizeof(struct videomode));
+			if (mode == NULL) return;
-			setmode = 1;
+			memcpy(&default_mode, mode, sizeof(default_mode));
 			if (!is_gx) sc->sc_setmode = 1;
 	} else {
 		/* make sure my_mode points at something sensible */
 		mach64_get_mode(sc, &default_mode);
 		if (default_mode.dot_clock == 0) {
 			memcpy(&default_mode, pick_mode_by_ref(width, height, 60),
 			    sizeof(default_mode));
+	}
 	sc->sc_my_mode = &default_mode;
 	if ((width == sc->sc_my_mode->hdisplay) &&
 	    (height == sc->sc_my_mode->vdisplay))
 		sc->sc_setmode = 0;
 	sc->bits_per_pixel = 8;
 	sc->virt_x = sc->sc_my_mode->hdisplay;
 	sc->virt_y = sc->sc_my_mode->vdisplay;
 	sc->max_x = sc->virt_x - 1;
 	sc->max_y = (sc->memsize * 1024) /
 	    (sc->virt_x * (sc->bits_per_pixel / 8)) - 1;
 	sc->color_depth = CRTC_PIX_WIDTH_8BPP;
 	mach64_init_engine(sc);
-	if (setmode)
+	if (sc->sc_setmode)
 		mach64_modeswitch(sc, sc->sc_my_mode);
 	aprint_normal_dev(sc->sc_dev,
 	    "initial resolution %dx%d at %d bpp\n",
 	    sc->sc_my_mode->hdisplay, sc->sc_my_mode->vdisplay,
 	    sc->bits_per_pixel);
 	wsfont_init();
 #ifdef GLYPHCACHE_DEBUG
 	/* shrink the screen so we can see part of the glyph cache */
 	sc->sc_my_mode->vdisplay -= 200;
 #endif
 @@ -717,27 +736,26 @@ mach64_attach(device_t parent, device_t
 		glyphcache_init(&sc->sc_gc, sc->sc_my_mode->vdisplay + 5,
 		    ((sc->memsize * 1024) / sc->sc_my_mode->hdisplay) -
 		      sc->sc_my_mode->vdisplay - 5,
 		    sc->sc_my_mode->hdisplay,
 		    ri->ri_font->fontwidth,
 		    ri->ri_font->fontheight,
 		    defattr);
 		wsdisplay_cnattach(&mach64_defaultscreen, ri, 0, 0, defattr);
 	} else {
 		/*
 		 * since we're not the console we can postpone the rest
 		 * until someone actually allocates a screen for us
 		 */
 		mach64_modeswitch(sc, sc->sc_my_mode);
 		if (mach64_console_screen.scr_ri.ri_rows == 0) {
 			/* do some minimal setup to avoid weirdnesses later */
 			vcons_init_screen(&sc->vd, &mach64_console_screen, 1,
 			    &defattr);
 		} else
 			(*ri->ri_ops.allocattr)(ri, 0, 0, 0, &defattr);
 		glyphcache_init(&sc->sc_gc, sc->sc_my_mode->vdisplay + 5,
 		    ((sc->memsize * 1024) / sc->sc_my_mode->hdisplay) -
 		      sc->sc_my_mode->vdisplay - 5,
 		    sc->sc_my_mode->hdisplay,
 		    ri->ri_font->fontwidth,
 		    ri->ri_font->fontheight,
 @@ -872,52 +890,54 @@ mach64_get_max_ramdac(struct mach64_soft
 	     (mach64_chip_rev & 0x07))
 		return 170000;
 	for (i = 0; i < __arraycount(mach64_info); i++)
 		if (mach64_chip_id == mach64_info[i].chip_id)
 			return mach64_info[i].ramdac_freq;
 	if (sc->bits_per_pixel == 8)
 		return 135000;
 	else
 		return 80000;
+}
 #if 0
 static void
 mach64_get_mode(struct mach64_softc *sc, struct videomode *mode)
+{
 	struct mach64_crtcregs crtc;
 	crtc.h_total_disp = regr(sc, CRTC_H_TOTAL_DISP);
 	crtc.h_sync_strt_wid = regr(sc, CRTC_H_SYNC_STRT_WID);
 	crtc.v_total_disp = regr(sc, CRTC_V_TOTAL_DISP);
 	crtc.v_sync_strt_wid = regr(sc, CRTC_V_SYNC_STRT_WID);
 	mode->htotal = ((crtc.h_total_disp & 0xffff) + 1) << 3;
 	mode->hdisplay = ((crtc.h_total_disp >> 16) + 1) << 3;
 	mode->hsync_start = ((crtc.h_sync_strt_wid & 0xffff) + 1) << 3;
 	mode->hsync_end = ((crtc.h_sync_strt_wid >> 16) << 3) +
 	    mode->hsync_start;
 	mode->vtotal = (crtc.v_total_disp & 0xffff) + 1;
 	mode->vdisplay = (crtc.v_total_disp >> 16) + 1;
 	mode->vsync_start = (crtc.v_sync_strt_wid & 0xffff) + 1;
 	mode->vsync_end = (crtc.v_sync_strt_wid >> 16) + mode->vsync_start;
 #ifdef MACHFB_DEBUG
 	printf("mach64_get_mode: %d %d %d %d %d %d %d %d\n",
 	    mode->hdisplay, mode->hsync_start, mode->hsync_end, mode->htotal,
 	    mode->vdisplay, mode->vsync_start, mode->vsync_end, mode->vtotal);
 #endif
+}
 #endif
 static int
 mach64_calc_crtcregs(struct mach64_softc *sc, struct mach64_crtcregs *crtc,
     struct videomode *mode)
+{
 	if (mode->dot_clock > sc->ramdac_freq)
 		/* Clock too high. */
 		return 1;
 	crtc->h_total_disp = (((mode->hdisplay >> 3) - 1) << 16) |
 	    ((mode->htotal >> 3) - 1);
 	crtc->h_sync_strt_wid =
 @@ -1137,27 +1157,30 @@ mach64_set_dsp(struct mach64_softc *sc)
 		fifo_depth = 24;
 	} else {
 		dsp_loop_latency = 2;
 		fifo_depth = 32;
+	}
 	dsp_precision = 0;
 	xclks_per_qw = (sc->mclk_fb_div * sc->vclk_post_div * 64 << 11) /
 	    (sc->vclk_fb_div * sc->mclk_post_div * sc->bits_per_pixel);
 	xclks_per_qw_m = (sc->mem_freq * 64 << 4) /
 		       (sc->vclk_freq * sc->bits_per_pixel);
 	printf("xclks_per_qw %d %d\n", xclks_per_qw >> 7, xclks_per_qw_m);
 	DPRINTF("xclks_per_qw %d %d\n", xclks_per_qw >> 7, xclks_per_qw_m);
 		DPRINTF("mem %dkHz v %dkHz\n", sc->mem_freq, sc->vclk_freq);
 	y = (xclks_per_qw * fifo_depth) >> 11;
 	while (y) {
 		y >>= 1;
 		dsp_precision++;
+	}
 	dsp_precision -= 5;
 	fifo_off = ((xclks_per_qw * (fifo_depth - 1)) >> 5) + (3 << 6);
 	switch (sc->memtype) {
 	case DRAM:
 	case EDO_DRAM:
 	case PSEUDO_EDO:
 @@ -1199,31 +1222,34 @@ mach64_set_dsp(struct mach64_softc *sc)
 	dsp_xclks_per_qw = xclks_per_qw >> dsp_precision;
 	dsp_on = fifo_on >> dsp_precision;
 	dsp_off = fifo_off >> dsp_precision;
 #ifdef MACHFB_DEBUG
 	printf("dsp_xclks_per_qw = %d, dsp_on = %d, dsp_off = %d,\n"
 	    "dsp_precision = %d, dsp_loop_latency = %d,\n"
 	    "mclk_fb_div = %d, vclk_fb_div = %d,\n"
 	    "mclk_post_div = %d, vclk_post_div = %d\n",
 	    dsp_xclks_per_qw, dsp_on, dsp_off, dsp_precision, dsp_loop_latency,
 	    sc->mclk_fb_div, sc->vclk_fb_div,
 	    sc->mclk_post_div, sc->vclk_post_div);
 #endif
 	DPRINTF("DSP_ON_OFF %08x\n", regr(sc, DSP_ON_OFF));
 	DPRINTF("DSP_CONFIG %08x\n", regr(sc, DSP_CONFIG));
 	regw(sc, DSP_ON_OFF, ((dsp_on << 16) & DSP_ON) | (dsp_off & DSP_OFF));
 	regw(sc, DSP_CONFIG, ((dsp_precision << 20) & DSP_PRECISION) |
 	    ((dsp_loop_latency << 16) & DSP_LOOP_LATENCY) |
 	    (dsp_xclks_per_qw & DSP_XCLKS_PER_QW));
 	DPRINTF("DSP_ON_OFF %08x\n", regr(sc, DSP_ON_OFF));
 	DPRINTF("DSP_CONFIG %08x\n", regr(sc, DSP_CONFIG));
+}
 static void
 mach64_set_pll(struct mach64_softc *sc, int clock)
+{
 	uint32_t q, clockreg;
 	int clockshift = sc->sc_clock << 1;
 	uint8_t reg, vclk_ctl;
 	q = (clock * sc->ref_div * 100) / (2 * sc->ref_freq);
 #ifdef MACHFB_DEBUG
 	printf("q = %d\n", q);
 #endif
 @@ -1240,35 +1266,35 @@ mach64_set_pll(struct mach64_softc *sc,
 		sc->log2_vclk_post_div = 1;
 	} else if (q > 3150) {
 		sc->vclk_post_div = 4;
 		sc->log2_vclk_post_div = 2;
 	} else if (q >= 1600) {
 		sc->vclk_post_div = 8;
 		sc->log2_vclk_post_div = 3;
 	} else {
 		aprint_error_dev(sc->sc_dev, "Warning: q < 1600\n");
 		sc->vclk_post_div = 8;
 		sc->log2_vclk_post_div = 3;
+	}
 	sc->vclk_fb_div = q * sc->vclk_post_div / 100;
-	aprint_error("post_div: %d log2_post_div: %d mclk_div: %d\n",
+	DPRINTF("post_div: %d log2_post_div: %d mclk_div: %d\n",
 	    sc->vclk_post_div, sc->log2_vclk_post_div, sc->mclk_fb_div);
 	vclk_ctl = regrb_pll(sc, PLL_VCLK_CNTL);
 	aprint_debug("vclk_ctl: %02x\n", vclk_ctl);
 	vclk_ctl |= PLL_VCLK_RESET;
 	regwb_pll(sc, PLL_VCLK_CNTL, vclk_ctl);
-	aprint_error("target: %d output: %d\n", clock,
+	DPRINTF("target: %d output: %d\n", clock,
 	    (2 * sc->ref_freq * sc->vclk_fb_div) /
 	    (sc->ref_div * sc->vclk_post_div));
 	regwb_pll(sc, MCLK_FB_DIV, sc->mclk_fb_div);
 	reg = regrb_pll(sc, VCLK_POST_DIV);
 	reg &= ~(3 << clockshift);
 	reg |= (sc->log2_vclk_post_div << clockshift);
 	regwb_pll(sc, VCLK_POST_DIV, reg);
 	regwb_pll(sc, VCLK0_FB_DIV + sc->sc_clock, sc->vclk_fb_div);
 	vclk_ctl &= ~PLL_VCLK_RESET;
 	regwb_pll(sc, PLL_VCLK_CNTL, vclk_ctl);
 @@ -1841,27 +1867,28 @@ mach64_ioctl(void *v, void *vs, u_long c
 		    sc->sc_pcitag, data);
 	case WSDISPLAYIO_SMODE: {
 		int new_mode = *(int*)data;
 		if (new_mode != sc->sc_mode) {
 			sc->sc_mode = new_mode;
 			if ((new_mode == WSDISPLAYIO_MODE_EMUL)
 			    && (ms != NULL))
+			{
 				/* restore initial video mode */
 				mach64_init(sc);
 				mach64_init_engine(sc);
 				mach64_init_lut(sc);
-				mach64_modeswitch(sc, sc->sc_my_mode);
+				if (sc->sc_setmode)
 					mach64_modeswitch(sc, sc->sc_my_mode);
 				mach64_clearscreen(sc);
 				glyphcache_wipe(&sc->sc_gc);
 				vcons_redraw_screen(ms);
+			}
+		}
+		}
 		return 0;
 	case WSDISPLAYIO_GET_EDID: {
 		struct wsdisplayio_edid_info *d = data;
 		return wsdisplayio_get_edid(sc->sc_dev, d);
+	}
 	case WSDISPLAYIO_GET_FBINFO: {