 @@ -1,14 +1,14 @@
-/*	$NetBSD: tcx.c,v 1.54 2014/07/16 17:58:35 macallan Exp $ */
+/*	$NetBSD: tcx.c,v 1.55 2014/07/22 04:55:51 macallan Exp $ */
 /*
  *  Copyright (c) 1996, 1998, 2009 The NetBSD Foundation, Inc.
  *  All rights reserved.
+ *
  *  This code is derived from software contributed to The NetBSD Foundation
  *  by Paul Kranenburg and Michael Lorenz.
+ *
  *  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.
 @@ -28,27 +28,27 @@
  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 /*
  * color display (TCX) driver.
+ *
  * Does not handle interrupts, even though they can occur.
+ *
  * XXX should defer colormap updates to vertical retrace interrupts
  */
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: tcx.c,v 1.54 2014/07/16 17:58:35 macallan Exp $");
+__KERNEL_RCSID(0, "$NetBSD: tcx.c,v 1.55 2014/07/22 04:55:51 macallan Exp $");
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/buf.h>
 #include <sys/device.h>
 #include <sys/ioctl.h>
 #include <sys/malloc.h>
 #include <sys/mman.h>
 #include <sys/tty.h>
 #include <sys/conf.h>
 #ifdef DEBUG
 #include <sys/proc.h>
 @@ -153,26 +153,27 @@ const struct cdevsw tcx_cdevsw = {
 /* frame buffer generic driver */
 static struct fbdriver tcx_fbdriver = {
 	tcx_unblank, tcxopen, tcxclose, tcxioctl, nopoll, tcxmmap,
 	nokqfilter
 };
 static void tcx_reset(struct tcx_softc *);
 static void tcx_loadcmap(struct tcx_softc *, int, int);
 static int	tcx_ioctl(void *, void *, u_long, void *, int, struct lwp *);
 static paddr_t	tcx_mmap(void *, void *, off_t, int);
 static void	tcx_init_cmap(struct tcx_softc *);
 static void	tcx_init_screen(void *, struct vcons_screen *, int, long *);
 static void	tcx_clearscreen(struct tcx_softc *, int);
 static void	tcx_copyrows(void *, int, int, int);
 static void	tcx_eraserows(void *, int, int, long);
 static void	tcx_putchar(void *, int, int, u_int, long);
 static void	tcx_set_video(struct tcx_softc *, int);
 static int	tcx_do_cursor(struct tcx_softc *, struct wsdisplay_cursor *);
 static void	tcx_set_cursor(struct tcx_softc *);
 struct wsdisplay_accessops tcx_accessops = {
 	tcx_ioctl,
 	tcx_mmap,
 	NULL,	/* vcons_alloc_screen */
 @@ -202,27 +203,27 @@ tcxmatch(device_t parent, cfdata_t cf, v
  * Attach a display.
  */
 void
 tcxattach(device_t parent, device_t self, void *args)
+{
 	struct tcx_softc *sc = device_private(self);
 	struct sbus_attach_args *sa = args;
 	struct wsemuldisplaydev_attach_args aa;
 	struct rasops_info *ri;
 	unsigned long defattr;
 	int node;
 	struct fbdevice *fb = &sc->sc_fb;
 	bus_space_handle_t bh;
-	int isconsole, i, j;
+	int isconsole;
 	uint32_t confreg;
 	sc->sc_dev = self;
 	sc->sc_bustag = sa->sa_bustag;
 	node = sa->sa_node;
 	sc->sc_cursor_x = 0x7fff;
 	sc->sc_cursor_y = 0x7fff;
 	sc->sc_hotspot_x = 0;
 	sc->sc_hotspot_y = 0;
 	fb->fb_driver = &tcx_fbdriver;
 	fb->fb_device = sc->sc_dev;
 @@ -367,37 +368,26 @@ tcxattach(device_t parent, device_t self
+	}
 	isconsole = fb_is_console(node);
 	confreg = bus_space_read_4(sa->sa_bustag, sc->sc_thc, THC_CONFIG);
 	aprint_normal_dev(self, "id %d, rev %d, sense %d\n",
 		(confreg & THC_CFG_FBID) >> THC_CFG_FBID_SHIFT,
 		(confreg & THC_CFG_REV) >> THC_CFG_REV_SHIFT,
 		(confreg & THC_CFG_SENSE) >> THC_CFG_SENSE_SHIFT
 	);
 	/* reset cursor & frame buffer controls */
 	tcx_reset(sc);
 	/* Initialize the default color map. */
 	j = 0;
 	for (i = 0; i < 256; i++) {
 		sc->sc_cmap_red[i] = rasops_cmap[j];
 		sc->sc_cmap_green[i] = rasops_cmap[j + 1];
 		sc->sc_cmap_blue[i] = rasops_cmap[j + 2];
 		j += 3;
 	tcx_loadcmap(sc, 0, 256);
 	if (!sc->sc_8bit)
 	    tcx_set_cursor(sc);
 	/* enable video */
 	confreg = bus_space_read_4(sa->sa_bustag, sc->sc_thc, THC_MISC);
 	confreg |= THC_MISC_VIDEN;
 	bus_space_write_4(sa->sa_bustag, sc->sc_thc, THC_MISC, confreg);
 	if (isconsole) {
 		aprint_error_dev(self, "(console)\n");
+	}
 	fb_attach(&sc->sc_fb, isconsole);
 @@ -405,26 +395,27 @@ tcxattach(device_t parent, device_t self
 	sc->sc_mode = WSDISPLAYIO_MODE_EMUL;
 	wsfont_init();
 	vcons_init(&sc->vd, sc, &tcx_defscreendesc, &tcx_accessops);
 	sc->vd.init_screen = tcx_init_screen;
 	vcons_init_screen(&sc->vd, &tcx_console_screen, 1, &defattr);
 	tcx_console_screen.scr_flags |= VCONS_SCREEN_IS_STATIC;
 	ri = &tcx_console_screen.scr_ri;
 	sc->sc_bg = ri->ri_devcmap[(defattr >> 16) & 0xff];
 	tcx_clearscreen(sc, 0);
 	tcx_init_cmap(sc);
 	tcx_defscreendesc.nrows = ri->ri_rows;
 	tcx_defscreendesc.ncols = ri->ri_cols;
 	tcx_defscreendesc.textops = &ri->ri_ops;
 	tcx_defscreendesc.capabilities = ri->ri_caps;
 	if(isconsole) {
 		wsdisplay_cnattach(&tcx_defscreendesc, ri, 0, 0, defattr);
 		vcons_replay_msgbuf(&tcx_console_screen);
+	}
 	aa.console = isconsole;
 	aa.scrdata = &tcx_screenlist;
 @@ -526,26 +517,43 @@ tcxioctl(dev_t dev, u_long cmd, void *da
  * Clean up hardware state (e.g., after bootup or after X crashes).
  */
 static void
 tcx_reset(struct tcx_softc *sc)
+{
 	uint32_t reg;
 	reg = bus_space_read_4(sc->sc_bustag, sc->sc_thc, THC_MISC);
 	reg |= THC_MISC_CURSRES;
 	bus_space_write_4(sc->sc_bustag, sc->sc_thc, THC_MISC, reg);
+}
 static void
 tcx_init_cmap(struct tcx_softc *sc)
+{
 	int i, j;
 	/* Initialize the default color map. */
 	j = 0;
 	for (i = 0; i < 256; i++) {
 		sc->sc_cmap_red[i] = rasops_cmap[j];
 		sc->sc_cmap_green[i] = rasops_cmap[j + 1];
 		sc->sc_cmap_blue[i] = rasops_cmap[j + 2];
 		j += 3;
+	}
 	tcx_loadcmap(sc, 0, 256);
+}
 static void
 tcx_loadcmap(struct tcx_softc *sc, int start, int ncolors)
+{
 	int i;
 	for (i = 0; i < ncolors; i++) {
 		bus_space_write_4(sc->sc_bustag, sc->sc_bt, DAC_ADDRESS,
 		    (start + i) << 24);
 		bus_space_write_4(sc->sc_bustag, sc->sc_bt, DAC_FB_LUT,
 		    sc->sc_cmap_red[i + start] << 24);
 		bus_space_write_4(sc->sc_bustag, sc->sc_bt, DAC_FB_LUT,
 		    sc->sc_cmap_green[i + start] << 24);
 		bus_space_write_4(sc->sc_bustag, sc->sc_bt, DAC_FB_LUT,
 		    sc->sc_cmap_blue[i + start] << 24);
 @@ -750,27 +758,27 @@ tcx_ioctl(void *v, void *vs, u_long cmd,
 			return tcx_getcmap(sc, (struct wsdisplay_cmap *)data);
 		case WSDISPLAYIO_PUTCMAP:
 			return tcx_putcmap(sc, (struct wsdisplay_cmap *)data);
 #endif
 		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)
+					{
-						tcx_loadcmap(sc, 0, 256);
+						tcx_init_cmap(sc);
 						tcx_clearscreen(sc, 0);
 						vcons_redraw_screen(ms);
 					} else if (!sc->sc_8bit)
 						tcx_clearscreen(sc, 3);
+				}
+			}
 			return 0;
 		case WSDISPLAYIO_GCURPOS:
 			if (sc->sc_8bit) {
 				return EOPNOTSUPP;
 			} else {
 				struct wsdisplay_curpos *cp = (void *)data;