Colormap ioctls, at last.

3 files changed, 347 insertions, 80 deletions

diff --git a/sys/arch/vax/vsa/gpx.c b/sys/arch/vax/vsa/gpx.c
index e203e774f51..e3d1d1493b3 100644
--- a/sys/arch/vax/vsa/gpx.c
+++ b/sys/arch/vax/vsa/gpx.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: gpx.c,v 1.9 2006/08/01 18:50:48 miod Exp $	*/
+/*	$OpenBSD: gpx.c,v 1.10 2006/08/05 22:04:53 miod Exp $	*/
 /*
  * Copyright (c) 2006 Miodrag Vallat.
  *
@@ -145,6 +145,7 @@ struct	gpx_screen {
 	u_int		ss_gpr;		/* font glyphs per row */
 	struct adder	*ss_adder;
 	void 		*ss_vdac;
+	u_int8_t	ss_cmap[256 * 3];
 	/* struct dc503reg *ss_cursor; */
 };
 
@@ -202,6 +203,10 @@ const struct wsdisplay_accessops gpx_accessops = {
 void	gpx_clear_screen(struct gpx_screen *);
 void	gpx_copyrect(struct gpx_screen *, int, int, int, int, int, int);
 void	gpx_fillrect(struct gpx_screen *, int, int, int, int, long, u_int);
+int	gpx_getcmap(struct gpx_screen *, struct wsdisplay_cmap *);
+void	gpx_loadcmap(struct gpx_screen *, int, int);
+int	gpx_putcmap(struct gpx_screen *, struct wsdisplay_cmap *);
+void	gpx_resetcmap(struct gpx_screen *);
 void	gpx_reset_viper(struct gpx_screen *);
 int	gpx_setup_screen(struct gpx_screen *);
 void	gpx_upload_font(struct gpx_screen *);
@@ -257,9 +262,9 @@ gpx_match(struct device *parent, void *vcf, void *aux)
 	if (depth != 0x00f0 && depth != 0x0080)
 		return (0);
 
-	if ((vax_confdata & (KA420_CFG_L3CON | KA420_CFG_MULTU)) == 0 &&
+	/* when already running as console, always fake things */
+	if ((vax_confdata & KA420_CFG_L3CON) == 0 &&
 	    cn_tab == &wsdisplay_cons) {
-		/* when already running as console, fake things */
 		struct vsbus_softc *sc = (void *)parent;
 		sc->sc_mask = 0x08;
 		scb_fake(0x44, oldvsbus ? 0x14 : 0x15);
@@ -268,7 +273,7 @@ gpx_match(struct device *parent, void *vcf, void *aux)
 		    GPX_ADDER_OFFSET, 1);
 		if (adder == NULL)
 			return (0);
-		adder->interrupt_enable = VSYNC;
+		adder->interrupt_enable = FRAME_SYNC;
 		DELAY(100000);	/* enough to get a retrace interrupt */
 		adder->interrupt_enable = 0;
 		vax_unmap_physmem((vaddr_t)adder, 1);
@@ -287,7 +292,7 @@ gpx_attach(struct device *parent, struct device *self, void *aux)
 	vaddr_t tmp;
 	extern struct consdev wsdisplay_cons;
 
-	console = (vax_confdata & (KA420_CFG_L3CON | KA420_CFG_MULTU)) == 0 &&
+	console = (vax_confdata & KA420_CFG_L3CON) == 0 &&
 	    cn_tab == &wsdisplay_cons;
 	if (console) {
 		scr = &gpx_consscr;
@@ -356,6 +361,8 @@ gpx_ioctl(void *v, u_long cmd, caddr_t data, int flag, struct proc *p)
 	struct gpx_softc *sc = v;
 	struct gpx_screen *ss = sc->sc_scr;
 	struct wsdisplay_fbinfo *wdf;
+	struct wsdisplay_cmap *cm;
+	int error;
 
 	switch (cmd) {
 	case WSDISPLAYIO_GTYPE:
@@ -371,8 +378,18 @@ gpx_ioctl(void *v, u_long cmd, caddr_t data, int flag, struct proc *p)
 		break;
 
 	case WSDISPLAYIO_GETCMAP:
+		cm = (struct wsdisplay_cmap *)data;
+		error = gpx_getcmap(ss, cm);
+		if (error != 0)
+			return (error);
+		break;
 	case WSDISPLAYIO_PUTCMAP:
-		break;	/* XXX TBD */
+		cm = (struct wsdisplay_cmap *)data;
+		error = gpx_putcmap(ss, cm);
+		if (error != 0)
+			return (error);
+		gpx_loadcmap(ss, cm->index, cm->count);
+		break;
 
 	case WSDISPLAYIO_GVIDEO:
 	case WSDISPLAYIO_SVIDEO:
@@ -630,6 +647,7 @@ gpx_reset_viper(struct gpx_screen *ss)
 {
 	int i;
 
+	ss->ss_adder->interrupt_enable = 0;
 	ss->ss_adder->command = CANCEL;
 	/* set monitor timing */
 	ss->ss_adder->x_scan_count_0 = 0x2800;
@@ -806,9 +824,7 @@ int
 gpx_setup_screen(struct gpx_screen *ss)
 {
 	struct rasops_info *ri = &ss->ss_ri;
-	const u_char *cmap;
 	int cookie;
-	int i, color12;
 
 	bzero(ri, sizeof(*ri));
 	ri->ri_depth = 8;	/* masquerade as a 8 bit device for rasops */
@@ -880,22 +896,7 @@ gpx_setup_screen(struct gpx_screen *ss)
 	/*
 	 * Initialize colormap.
 	 */
-	gpx_wait(ss, FRAME_SYNC);
-	cmap = rasops_cmap;
-	if (ss->ss_depth == 8) {
-		struct ramdac8 *rd = ss->ss_vdac;
-		rd->address = 0;
-		for (i = 0; i < 256 * 3; i++)
-			rd->cmapdata = *cmap++;
-	} else {
-		struct ramdac4 *rd = ss->ss_vdac;
-		for (i = 0; i < 16; i++) {
-			color12  = (*cmap++ >> 4) << 8;
-			color12 |= (*cmap++ >> 4) << 4;
-			color12 |= (*cmap++ >> 4) << 0;
-			rd->colormap[i] = color12;
-		}
-	}
+	gpx_resetcmap(ss);
 
 	/*
 	 * Clear display (including non-visible area), in 864 lines chunks.
@@ -1067,6 +1068,113 @@ gpx_fillrect(struct gpx_screen *ss, int x, int y, int dx, int dy, long attr,
 }
 
 /*
+ * Colormap handling routines
+ */
+
+int
+gpx_getcmap(struct gpx_screen *ss, struct wsdisplay_cmap *cm)
+{
+	u_int index = cm->index, count = cm->count, i;
+	u_int colcount = 1 << ss->ss_depth;
+	int error;
+	u_int8_t ramp[256], *c, *r;
+
+	if (index >= colcount || count > colcount - index)
+		return (EINVAL);
+
+	/* extract reds */
+	c = ss->ss_cmap + 0 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c << (8 - ss->ss_depth), c += 3;
+	if ((error = copyout(ramp, cm->red, count)) != 0)
+		return (error);
+
+	/* extract greens */
+	c = ss->ss_cmap + 1 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c << (8 - ss->ss_depth), c += 3;
+	if ((error = copyout(ramp, cm->green, count)) != 0)
+		return (error);
+
+	/* extract blues */
+	c = ss->ss_cmap + 2 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c << (8 - ss->ss_depth), c += 3;
+	if ((error = copyout(ramp, cm->blue, count)) != 0)
+		return (error);
+
+	return (0);
+}
+
+int
+gpx_putcmap(struct gpx_screen *ss, struct wsdisplay_cmap *cm)
+{
+	u_int index = cm->index, count = cm->count;
+	u_int colcount = 1 << ss->ss_depth;
+	int i, error;
+	u_int8_t r[256], g[256], b[256], *nr, *ng, *nb, *c;
+
+	if (index >= colcount || count > colcount - index)
+		return (EINVAL);
+
+	if ((error = copyin(cm->red, r, count)) != 0)
+		return (error);
+	if ((error = copyin(cm->green, g, count)) != 0)
+		return (error);
+	if ((error = copyin(cm->blue, b, count)) != 0)
+		return (error);
+
+	nr = r, ng = g, nb = b;
+	c = ss->ss_cmap + index * 3;
+	for (i = count; i != 0; i--) {
+		*c++ = *nr++ >> (8 - ss->ss_depth);
+		*c++ = *ng++ >> (8 - ss->ss_depth);
+		*c++ = *nb++ >> (8 - ss->ss_depth);
+	}
+
+	return (0);
+}
+
+void
+gpx_loadcmap(struct gpx_screen *ss, int from, int count)
+{
+	u_int8_t *cmap = ss->ss_cmap;
+	int i, color12;
+
+	gpx_wait(ss, FRAME_SYNC);
+	if (ss->ss_depth == 8) {
+		struct ramdac8 *rd = ss->ss_vdac;
+
+		cmap += from * 3;
+		rd->address = from;
+		for (i = 0; i < count * 3; i++)
+			rd->cmapdata = *cmap++;
+	} else {
+		struct ramdac4 *rd = ss->ss_vdac;
+
+		cmap = ss->ss_cmap + from;
+		for (i = from; i < from + count; i++) {
+			color12  = *cmap++ << 8;
+			color12 |= *cmap++ << 4;
+			color12 |= *cmap++ << 0;
+			rd->colormap[i] = color12;
+		}
+	}
+}
+
+void
+gpx_resetcmap(struct gpx_screen *ss)
+{
+	if (ss->ss_depth == 8)
+		bcopy(rasops_cmap, ss->ss_cmap, sizeof(ss->ss_cmap));
+	else {
+		bcopy(rasops_cmap, ss->ss_cmap, 8 * 3);
+		bcopy(rasops_cmap + 0xf8 * 3, ss->ss_cmap + 8 * 3, 8 * 3);
+	}
+	gpx_loadcmap(ss, 0, 1 << ss->ss_depth);
+}
+
+/*
  * Console support code
  */
 
diff --git a/sys/arch/vax/vsa/lcg.c b/sys/arch/vax/vsa/lcg.c
index e961ebe1c06..bc2f956257b 100644
--- a/sys/arch/vax/vsa/lcg.c
+++ b/sys/arch/vax/vsa/lcg.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: lcg.c,v 1.6 2006/08/03 18:46:50 miod Exp $	*/
+/*	$OpenBSD: lcg.c,v 1.7 2006/08/05 22:04:53 miod Exp $	*/
 /*
  * Copyright (c) 2006 Miodrag Vallat.
  *
@@ -85,6 +85,7 @@ struct	lcg_screen {
 	caddr_t		ss_addr;		/* frame buffer address */
 	vaddr_t		ss_reg;
 	volatile u_int8_t *ss_lut;
+	u_int8_t	ss_cmap[256 * 3];
 };
 
 /* for console */
@@ -139,9 +140,11 @@ const struct wsdisplay_accessops lcg_accessops = {
 };
 
 int	lcg_alloc_attr(void *, int, int, int, long *);
+int	lcg_getcmap(struct lcg_screen *, struct wsdisplay_cmap *);
+void	lcg_loadcmap(struct lcg_screen *, int, int);
 int	lcg_probe_screen(u_int32_t, u_int *, u_int *);
+int	lcg_putcmap(struct lcg_screen *, struct wsdisplay_cmap *);
 void	lcg_resetcmap(struct lcg_screen *);
-void	lcg_set_lut_entry(volatile u_int8_t *, const u_char *, u_int, u_int);
 int	lcg_setup_screen(struct lcg_screen *);
 
 #define	lcg_read_reg(ss, regno) \
@@ -397,12 +400,6 @@ lcg_setup_screen(struct lcg_screen *ss)
 	ri->ri_hw = ss;
 
 	/*
-	 * We can let rasops select our font here, as we do not need to
-	 * use a font with a different bit order than rasops' defaults,
-	 * unlike smg.
-	 */
-
-	/*
 	 * Ask for an unholy big display, rasops will trim this to more
 	 * reasonable values.
 	 */
@@ -432,6 +429,8 @@ lcg_ioctl(void *v, u_long cmd, caddr_t data, int flag, struct proc *p)
 	struct lcg_softc *sc = v;
 	struct lcg_screen *ss = sc->sc_scr;
 	struct wsdisplay_fbinfo *wdf;
+	struct wsdisplay_cmap *cm;
+	int error;
 
 	switch (cmd) {
 	case WSDISPLAYIO_GTYPE:
@@ -451,8 +450,18 @@ lcg_ioctl(void *v, u_long cmd, caddr_t data, int flag, struct proc *p)
 		break;
 
 	case WSDISPLAYIO_GETCMAP:
+		cm = (struct wsdisplay_cmap *)data;
+		error = lcg_getcmap(ss, cm);
+		if (error != 0)
+			return (error);
+		break;
 	case WSDISPLAYIO_PUTCMAP:
-		break;	/* XXX TBD */
+		cm = (struct wsdisplay_cmap *)data;
+		error = lcg_putcmap(ss, cm);
+		if (error != 0)
+			return (error);
+		lcg_loadcmap(ss, cm->index, cm->count);
+		break;
 
 	case WSDISPLAYIO_GVIDEO:
 	case WSDISPLAYIO_SVIDEO:
@@ -542,50 +551,105 @@ lcg_alloc_attr(void *cookie, int fg, int bg, int flg, long *attr)
 }
 
 /*
- * Fill the given colormap (LUT) entry.
+ * Colormap handling routines
  */
-void
-lcg_set_lut_entry(volatile u_int8_t *lutptr, const u_char *cmap, u_int idx,
-    u_int shift)
+
+int
+lcg_getcmap(struct lcg_screen *ss, struct wsdisplay_cmap *cm)
 {
-	lutptr++;
-	*lutptr++ = idx;
-	*lutptr++ = 1;
-	*lutptr++ = (*cmap++) >> shift;
-	*lutptr++ = 1;
-	*lutptr++ = (*cmap++) >> shift;
-	*lutptr++ = 1;
-	*lutptr++ = (*cmap++) >> shift;
+	u_int index = cm->index, count = cm->count, i;
+	u_int colcount = 1 << ss->ss_depth;
+	int error;
+	u_int8_t ramp[256], *c, *r;
+
+	if (index >= colcount || count > colcount - index)
+		return (EINVAL);
+
+	/* extract reds */
+	c = ss->ss_cmap + 0 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c, c += 3;
+	if ((error = copyout(ramp, cm->red, count)) != 0)
+		return (error);
+
+	/* extract greens */
+	c = ss->ss_cmap + 1 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c, c += 3;
+	if ((error = copyout(ramp, cm->green, count)) != 0)
+		return (error);
+
+	/* extract blues */
+	c = ss->ss_cmap + 2 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c, c += 3;
+	if ((error = copyout(ramp, cm->blue, count)) != 0)
+		return (error);
+
+	return (0);
 }
 
+int
+lcg_putcmap(struct lcg_screen *ss, struct wsdisplay_cmap *cm)
+{
+	u_int index = cm->index, count = cm->count;
+	u_int colcount = 1 << ss->ss_depth;
+	int i, error;
+	u_int8_t r[256], g[256], b[256], *nr, *ng, *nb, *c;
+
+	if (index >= colcount || count > colcount - index)
+		return (EINVAL);
+
+	if ((error = copyin(cm->red, r, count)) != 0)
+		return (error);
+	if ((error = copyin(cm->green, g, count)) != 0)
+		return (error);
+	if ((error = copyin(cm->blue, b, count)) != 0)
+		return (error);
+
+	nr = r, ng = g, nb = b;
+	c = ss->ss_cmap + index * 3;
+	for (i = count; i != 0; i--) {
+		*c++ = *nr++;
+		*c++ = *ng++;
+		*c++ = *nb++;
+	}
+
+	return (0);
+}
+
+/* Fill the given colormap (LUT) entry.  */
+#define lcg_set_lut_entry(lutptr, cmap, idx, shift)			\
+do {									\
+	(lutptr)++;							\
+	*(lutptr)++ = (idx);						\
+	*(lutptr)++ = 1;						\
+	*(lutptr)++ = (*(cmap)++) >> (shift);				\
+	*(lutptr)++ = 1;						\
+	*(lutptr)++ = (*(cmap)++) >> (shift);				\
+	*(lutptr)++ = 1;						\
+	*(lutptr)++ = (*(cmap)++) >> (shift);				\
+} while (0)
+
 void
-lcg_resetcmap(struct lcg_screen *ss)
+lcg_loadcmap(struct lcg_screen *ss, int from, int count)
 {
-	const u_char *color;
+	const u_int8_t *cmap;
 	u_int i;
 	volatile u_int8_t *lutptr;
 	u_int32_t vidcfg;
 
+	/* partial updates ignored for now */
 	vidcfg = lcg_read_reg(ss, LCG_REG_VIDEO_CONFIG);
-	color = rasops_cmap;
+	cmap = ss->ss_cmap;
 	lutptr = ss->ss_lut;
 	if (ss->ss_depth == 8) {
 		for (i = 0; i < 256; i++) {
-			lcg_set_lut_entry(lutptr, color, i, 0);
-			lutptr += 8;
-			color += 3;
+			lcg_set_lut_entry(lutptr, cmap, i, 0);
 		}
 	} else {
-		for (i = 0; i < 8; i++) {
-			lcg_set_lut_entry(lutptr, color, i, 4);
-			lutptr += 8;
-			color += 3;
-		}
-		color = rasops_cmap + 0xf8 * 3;
-		for (i = 0xf8; i < 0x100; i++) {
-			lcg_set_lut_entry(lutptr, color, i & 0x0f, 4);
-			lutptr += 8;
-			color += 3;
+		for (i = 0; i < 16; i++) {
+			lcg_set_lut_entry(lutptr, cmap, i, 4);
 		}
 	}
 	vidcfg &= ~((1 << 5) | (3 << 8) | (1 << 11));
@@ -600,6 +664,18 @@ lcg_resetcmap(struct lcg_screen *ss)
 	lcg_write_reg(ss, LCG_REG_LUT_CONSOLE_SEL, 0);
 }
 
+void
+lcg_resetcmap(struct lcg_screen *ss)
+{
+	if (ss->ss_depth == 8)
+		bcopy(rasops_cmap, ss->ss_cmap, sizeof(ss->ss_cmap));
+	else {
+		bcopy(rasops_cmap, ss->ss_cmap, 8 * 3);
+		bcopy(rasops_cmap + 0xf8 * 3, ss->ss_cmap + 8 * 3, 8 * 3);
+	}
+	lcg_loadcmap(ss, 0, 1 << ss->ss_depth);
+}
+
 /*
  * Console support code
  */
diff --git a/sys/arch/vax/vsa/lcspx.c b/sys/arch/vax/vsa/lcspx.c
index e502bc17ce9..84b63e35120 100644
--- a/sys/arch/vax/vsa/lcspx.c
+++ b/sys/arch/vax/vsa/lcspx.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: lcspx.c,v 1.4 2006/08/03 18:46:07 miod Exp $	*/
+/*	$OpenBSD: lcspx.c,v 1.5 2006/08/05 22:04:53 miod Exp $	*/
 /*
  * Copyright (c) 2006 Miodrag Vallat.
  *
@@ -87,6 +87,7 @@ struct	lcspx_screen {
 	caddr_t		ss_addr;		/* frame buffer address */
 	volatile u_int8_t *ss_ramdac[4];
 	vaddr_t		ss_reg;
+	u_int8_t	ss_cmap[256 * 3];
 };
 
 #define	lcspx_reg_read(ss, reg) \
@@ -144,6 +145,9 @@ const struct wsdisplay_accessops lcspx_accessops = {
 	NULL	/* burner */
 };
 
+int	lcspx_getcmap(struct lcspx_screen *, struct wsdisplay_cmap *);
+void	lcspx_loadcmap(struct lcspx_screen *, int, int);
+int	lcspx_putcmap(struct lcspx_screen *, struct wsdisplay_cmap *);
 void	lcspx_resetcmap(struct lcspx_screen *);
 int	lcspx_setup_screen(struct lcspx_screen *);
 
@@ -276,10 +280,10 @@ lcspx_setup_screen(struct lcspx_screen *ss)
 	ri->ri_hw = ss;
 
 	/*
-	 * We can let rasops select our font here, as we do not need to
-	 * use a font with a different bit order than rasops' defaults,
-	 * unlike smg.
+	 * Enable all planes for reading and writing
 	 */
+	lcspx_reg_write(ss, 0x1170, 0xffffffff);
+	lcspx_reg_write(ss, 0x1174, 0xffffffff);
 
 	/*
 	 * Ask for an unholy big display, rasops will trim this to more
@@ -288,12 +292,6 @@ lcspx_setup_screen(struct lcspx_screen *ss)
 	if (rasops_init(ri, 160, 160) != 0)
 		return (-1);
 
-	/*
-	 * Enable all planes for reading and writing
-	 */
-	lcspx_reg_write(ss, 0x1170, 0xffffffff);
-	lcspx_reg_write(ss, 0x1174, 0xffffffff);
-
 	lcspx_resetcmap(ss);
 
 	lcspx_stdscreen.ncols = ri->ri_cols;
@@ -312,6 +310,8 @@ lcspx_ioctl(void *v, u_long cmd, caddr_t data, int flag, struct proc *p)
 	struct lcspx_softc *sc = v;
 	struct lcspx_screen *ss = sc->sc_scr;
 	struct wsdisplay_fbinfo *wdf;
+	struct wsdisplay_cmap *cm;
+	int error;
 
 	switch (cmd) {
 	case WSDISPLAYIO_GTYPE:
@@ -331,8 +331,18 @@ lcspx_ioctl(void *v, u_long cmd, caddr_t data, int flag, struct proc *p)
 		break;
 
 	case WSDISPLAYIO_GETCMAP:
+		cm = (struct wsdisplay_cmap *)data;
+		error = lcspx_getcmap(ss, cm);
+		if (error != 0)
+			return (error);
+		break;
 	case WSDISPLAYIO_PUTCMAP:
-		break;	/* XXX TBD */
+		cm = (struct wsdisplay_cmap *)data;
+		error = lcspx_putcmap(ss, cm);
+		if (error != 0)
+			return (error);
+		lcspx_loadcmap(ss, cm->index, cm->count);
+		break;
 
 	case WSDISPLAYIO_GVIDEO:
 	case WSDISPLAYIO_SVIDEO:
@@ -388,14 +398,80 @@ lcspx_show_screen(void *v, void *cookie, int waitok,
 	return (0);
 }
 
+/*
+ * Colormap handling routines
+ */
+
+int
+lcspx_getcmap(struct lcspx_screen *ss, struct wsdisplay_cmap *cm)
+{
+	u_int index = cm->index, count = cm->count, i;
+	int error;
+	u_int8_t ramp[256], *c, *r;
+
+	if (index >= 256 || count > 256 - index)
+		return (EINVAL);
+
+	/* extract reds */
+	c = ss->ss_cmap + 0 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c, c += 3;
+	if ((error = copyout(ramp, cm->red, count)) != 0)
+		return (error);
+
+	/* extract greens */
+	c = ss->ss_cmap + 1 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c, c += 3;
+	if ((error = copyout(ramp, cm->green, count)) != 0)
+		return (error);
+
+	/* extract blues */
+	c = ss->ss_cmap + 2 + index * 3;
+	for (i = count, r = ramp; i != 0; i--)
+		*r++ = *c, c += 3;
+	if ((error = copyout(ramp, cm->blue, count)) != 0)
+		return (error);
+
+	return (0);
+}
+
+int
+lcspx_putcmap(struct lcspx_screen *ss, struct wsdisplay_cmap *cm)
+{
+	u_int index = cm->index, count = cm->count;
+	int i, error;
+	u_int8_t r[256], g[256], b[256], *nr, *ng, *nb, *c;
+
+	if (index >= 256 || count > 256 - index)
+		return (EINVAL);
+
+	if ((error = copyin(cm->red, r, count)) != 0)
+		return (error);
+	if ((error = copyin(cm->green, g, count)) != 0)
+		return (error);
+	if ((error = copyin(cm->blue, b, count)) != 0)
+		return (error);
+
+	nr = r, ng = g, nb = b;
+	c = ss->ss_cmap + index * 3;
+	for (i = count; i != 0; i--) {
+		*c++ = *nr++;
+		*c++ = *ng++;
+		*c++ = *nb++;
+	}
+
+	return (0);
+}
+
 void
-lcspx_resetcmap(struct lcspx_screen *ss)
+lcspx_loadcmap(struct lcspx_screen *ss, int from, int count)
 {
-	const u_char *color;
-	u_int i;
+	u_int8_t *cmap = ss->ss_cmap;
+	int i;
 
-	color = rasops_cmap;
-	for (i = 0; i < 256; i++) {
+	cmap += from * 3;
+	for (i = from; i < from + count; i++) {
 		/*
 		 * Reprogram the index every iteration, because the RAMDAC
 		 * may not be in autoincrement mode. XXX fix this
@@ -403,12 +479,19 @@ lcspx_resetcmap(struct lcspx_screen *ss)
 		*(ss->ss_ramdac[BT463_REG_ADDR_LOW]) = i & 0xff;
 		*(ss->ss_ramdac[BT463_REG_ADDR_HIGH]) = i >> 8;
 
-		*(ss->ss_ramdac[BT463_REG_CMAP_DATA]) = *color++;
-		*(ss->ss_ramdac[BT463_REG_CMAP_DATA]) = *color++;
-		*(ss->ss_ramdac[BT463_REG_CMAP_DATA]) = *color++;
+		*(ss->ss_ramdac[BT463_REG_CMAP_DATA]) = *cmap++;
+		*(ss->ss_ramdac[BT463_REG_CMAP_DATA]) = *cmap++;
+		*(ss->ss_ramdac[BT463_REG_CMAP_DATA]) = *cmap++;
 	}
 }
 
+void
+lcspx_resetcmap(struct lcspx_screen *ss)
+{
+	bcopy(rasops_cmap, ss->ss_cmap, sizeof(ss->ss_cmap));
+	lcspx_loadcmap(ss, 0, 256);
+}
+
 /*
  * Console support code
  */