- Add some more x86 instructions to emulate,

- emulate VGA read mode 0,
- emulate VGA write mode 1,
- minor cleanup.

Protel's Easytrax, a free PCB layout program, almost runs now; there are
still some problems with the keyboard emulation, but the graphics are fine
(albeit a bit slow).

4 files changed, 267 insertions, 136 deletions

diff --git a/usr.bin/doscmd/cpu.c b/usr.bin/doscmd/cpu.c
index e0d58a7..ea60ff1 100644
--- a/usr.bin/doscmd/cpu.c
+++ b/usr.bin/doscmd/cpu.c
@@ -29,14 +29,16 @@
 #include "doscmd.h"
 #include "video.h"
 
-static u_int32_t decode_modrm(u_int8_t *, u_int16_t, regcontext_t *, int *);
-static u_int8_t reg8(u_int8_t c, regcontext_t *);
-static u_int16_t reg16(u_int8_t c, regcontext_t *);
+static u_int32_t	decode_modrm(u_int8_t *, u_int16_t,
+				     regcontext_t *, int *);
+static u_int8_t		*reg8(u_int8_t c, regcontext_t *);
+static u_int16_t	*reg16(u_int8_t c, regcontext_t *);
 #if 0
-static u_int32_t reg32(u_int8_t c, regcontext_t *);
+static u_int32_t	*reg32(u_int8_t c, regcontext_t *);
 #endif
-static void write_byte(u_int32_t, u_int8_t);
-static void write_word(u_int32_t, u_int16_t);
+static u_int8_t		read_byte(u_int32_t);
+static void		write_byte(u_int32_t, u_int8_t);
+static void		write_word(u_int32_t, u_int16_t);
 
 /*
 ** Hardware /0 interrupt
@@ -120,90 +122,163 @@ emu_instr(regcontext_t *REGS)
     int prefix = 1;
     u_int8_t *cs = (u_int8_t *)(R_CS << 4);
     int ip = R_IP;
-    int instrlen;
-    int dir;
-    u_int16_t value;
-    u_int16_t seg = R_DS;
-    u_int32_t addr, endaddr;
+    int dir, i, instrlen;
+    u_int8_t *r8;
+    u_int8_t val8;
+    u_int16_t val16;
+    u_int16_t *seg = &R_DS;
+    u_int32_t addr, toaddr;
     
     while (prefix) {
 	prefix = 0;
 	switch (cs[ip]) {
+	case 0x08:		/* or r/m8, r8 */
+	    addr = decode_modrm(cs + ip, *seg, REGS, &instrlen);
+	    r8 = reg8(cs[ip + 1], REGS);
+	    val8 = read_byte(addr) | *r8;
+	    write_byte(addr, val8);
+	    /* clear carry and overflow; check zero, sign, parity */
+	    R_EFLAGS &= ~PSL_C | ~PSL_V;
+	    if (val8 == 0)
+		R_EFLAGS |= PSL_Z;
+	    if (val8 % 2 != 0)
+		R_EFLAGS |= PSL_PF;
+	    if (val8 & 0x80)
+		R_EFLAGS |= PSL_N;
+	    ip += 2 + instrlen;
+	    break;
+	case 0x22:		/* and r8, r/m8 */
+	    addr = decode_modrm(cs + ip, *seg, REGS, &instrlen);
+	    r8 = reg8(cs[ip + 1], REGS);
+	    *r8 &= read_byte(addr);
+	    /* clear carry and overflow; check zero, sign, parity */
+	    R_EFLAGS &= ~PSL_C | ~PSL_V;
+	    if (*r8 == 0)
+		R_EFLAGS |= PSL_Z;
+	    if (*r8 % 2 != 0)
+		R_EFLAGS |= PSL_PF;
+	    if (*r8 & 0x80)
+		R_EFLAGS |= PSL_N;
+	    ip += 2 + instrlen;
+	    break;
 	case 0x26:		/* Segment Override ES */
-	    seg = R_ES;
+	    seg = &R_ES;
 	    prefix = 1;
 	    ip++;
 	    break;
 	case 0x2e:		/* Segment Override CS */
-	    seg = R_CS;
+	    seg = &R_CS;
 	    prefix = 1;
 	    ip++;
 	    break;
 	case 0x36:		/* Segment Override SS */
-	    seg = R_SS;
+	    seg = &R_SS;
 	    prefix = 1;
 	    ip++;
 	    break;
 	case 0x3e:		/* Segment Override DS */
-	    seg = R_DS;
+	    seg = &R_DS;
 	    prefix = 1;
 	    ip++;
 	    break;
 	case 0x64:		/* Segment Override FS */
-	    seg = R_FS;
+	    seg = &R_FS;
 	    prefix = 1;
 	    ip++;
 	    break;
 	case 0x65:		/* Segment Override GS */
-	    seg = R_GS;
+	    seg = &R_GS;
 	    prefix = 1;
 	    ip++;
 	    break;
 	case 0x88:		/* mov r/m8, r8 */
-	    addr = decode_modrm(cs + ip, seg, REGS, &instrlen);
-	    write_byte(addr, reg8(cs[ip + 1], REGS));
+	    addr = decode_modrm(cs + ip, *seg, REGS, &instrlen);
+	    write_byte(addr, *reg8(cs[ip + 1], REGS));
+	    ip += 2 + instrlen;
+	    break;
+	case 0x8a:		/* mov r8, r/m8 */
+	    addr = decode_modrm(cs + ip, *seg, REGS, &instrlen);
+	    r8 = reg8(cs[ip + 1], REGS);
+	    *r8 = read_byte(addr);
 	    ip += 2 + instrlen;
 	    break;
 	case 0xc6:		/* mov r/m8, imm8 */
-	    addr = decode_modrm(cs + ip, seg, REGS, &instrlen);
+	    addr = decode_modrm(cs + ip, *seg, REGS, &instrlen);
 	    write_byte(addr, cs[ip + 2 + instrlen]);
 	    ip += 2 + instrlen + 1;
 	    break;
 	case 0xc7:		/* mov r/m32/16, imm32/16 */
-	    addr = decode_modrm(cs + ip, seg, REGS, &instrlen);
-	    value = *(u_int16_t *)&cs[ip + 2 + instrlen];
-	    write_word(addr, value);
+	    addr = decode_modrm(cs + ip, *seg, REGS, &instrlen);
+	    val16 = *(u_int16_t *)&cs[ip + 2 + instrlen];
+	    write_word(addr, val16);
 	    ip += 2 + instrlen + 2;
 	    break;
+	case 0xa4:		/* movs m8, m8 */
+	    write_byte(MAKEPTR(R_ES, R_DI), read_byte(MAKEPTR(*seg, R_SI)));
+	    dir = (R_EFLAGS & PSL_D) ? -1 : 1;
+	    R_DI += dir;
+	    R_SI += dir;
+	    ip++;
+	    break;
+	case 0xaa:		/* stos m8 */
+	    addr = MAKEPTR(R_ES, R_DI);
+	    write_byte(addr, R_AL);
+	    R_DI += (R_EFLAGS & PSL_D) ? -1 : 1;
+	    ip++;
+	    break;
 	case 0xab:		/* stos m32/16*/
+	    addr = MAKEPTR(R_ES, R_DI);
+	    write_word(addr, R_AX);
+	    R_DI += (R_EFLAGS & PSL_D) ? -2 : 2;
+	    ip++;
 	    break;
 	case 0xf3:		/* rep */
 	    switch (cs[++ip]) {
-	    case 0xab:		/* stos m32/16 */
-		value = R_AX;
+	    case 0xa4:		/* movs m8, m8 */
+		/* XXX Possible optimization: if both source and target
+		   addresses lie within the video memory and write mode 1 is
+		   selected, we can use memcpy(). */
+		dir = (R_EFLAGS & PSL_D) ? -1 : 1;
+		addr = MAKEPTR(R_ES, R_DI);
+		toaddr = MAKEPTR(*seg, R_SI);
+		for (i = R_CX; i > 0; i--) {
+		    write_byte(addr, read_byte(toaddr));
+		    addr += dir;
+		    toaddr += dir;
+		}
+		PUTPTR(R_ES, R_DI, addr);
+		PUTPTR(*seg, R_SI, toaddr);
+		ip++;
+		break;
+	    case 0xaa:		/* stos m8 */
 		/* direction */
 		dir = (R_EFLAGS & PSL_D) ? -1 : 1;
 		addr = MAKEPTR(R_ES, R_DI);
-		endaddr = MAKEPTR(R_ES, R_DI) + dir * R_CX;
-		if (addr <= endaddr)
-		    while (addr <= endaddr) {
-			write_word(addr, value);
-			addr += 2;
-		    }
-		else
-		    while (addr >= endaddr) {
-			write_word(addr, value);
-			addr -= 2;
-		    }
-		ip += 2;
+		for (i = R_CX; i > 0; i--) {
+		    write_byte(addr, R_AL);
+		    addr += dir;
+		}
+		PUTPTR(R_ES, R_DI, addr);
+		ip++;
+		break;
+	    case 0xab:		/* stos m32/16 */
+		/* direction */
+		dir = (R_EFLAGS & PSL_D) ? -2 : 2;
+		addr = MAKEPTR(R_ES, R_DI);
+		for (i = R_CX; i > 0; i--) {
+		    write_word(addr, R_AX);
+		    addr += dir;
+		}
+		PUTPTR(R_ES, R_DI, addr);
+		ip++;
 		break;
 	    default:
-		R_IP = ip--;	/* Move IP back to the 'rep' instruction */
+		R_IP = --ip;	/* Move IP back to the 'rep' instruction. */
 		return -1;
 	    }
 	    break;
 	default:
-	    /* unknown instruction, get out of here and let trap.c:sigbus()
+	    /* Unknown instruction, get out of here and let trap.c:sigbus()
 	       catch it. */
 	    return -1;
 	}
@@ -243,9 +318,9 @@ decode_modrm(u_int8_t *c, u_int16_t seg, regcontext_t *REGS, int *instrlen)
 	break;
     case 0xc0:			/* reg in R/M */
 	if (c[0] & 1)		/* 16-bit reg */
-	    return reg16(c[1], REGS);
+	    return *reg16(c[1], REGS);
 	else			/* 8-bit reg */
-	    return reg8(c[1], REGS);
+	    return *reg8(c[1], REGS);
 	break;
     }
     
@@ -284,50 +359,63 @@ decode_modrm(u_int8_t *c, u_int16_t seg, regcontext_t *REGS, int *instrlen)
     return addr;
 }
 
-static u_int8_t
+static u_int8_t *
 reg8(u_int8_t c, regcontext_t *REGS)
 {
-    u_int8_t	r8[] = {R_AL, R_CL, R_DL, R_BL, R_AH, R_CH, R_DH, R_BH};
+    u_int8_t *r8[] = {&R_AL, &R_CL, &R_DL, &R_BL,
+		      &R_AH, &R_CH, &R_DH, &R_BH};
 
     /* select 'rrr' bits in ModR/M */
     return r8[(c & 0x34) >> 3];
 }
 
-static u_int16_t
+static u_int16_t *
 reg16(u_int8_t c, regcontext_t *REGS)
 {
-    u_int16_t	r16[] = {R_AX, R_CX, R_DX, R_BX, R_SP, R_BP, R_SI, R_DI};
+    u_int16_t *r16[] = {&R_AX, &R_CX, &R_DX, &R_BX,
+			&R_SP, &R_BP, &R_SI, &R_DI};
 
     return r16[(c & 0x34) >> 3];
 }
 
 #if 0
 /* not yet */
-static u_int32_t
+static u_int32_t *
 reg32(u_int8_t c, regcontext_t *REGS)
 {
-    u_int32_t	r32[] = {R_EAX, R_ECX, R_EDX, R_EBX,
-			 R_ESP, R_EBP, R_ESI, R_EDI};
+    u_int32_t *r32[] = {&R_EAX, &R_ECX, &R_EDX, &R_EBX,
+			&R_ESP, &R_EBP, &R_ESI, &R_EDI};
 
     return r32[(c & 0x34) >> 3];
 }
 #endif
 
+/* Read an 8-bit value from the location specified by 'addr'. If 'addr' lies
+   within the video memory, we call 'video.c:vga_read()'. */
+static u_int8_t
+read_byte(u_int32_t addr)
+{
+    if (addr >= 0xa0000 && addr < 0xb0000)
+	return vga_read(addr);
+    else
+	return *(u_int8_t *)addr;
+}
+
 /* Write an 8-bit value to the location specified by 'addr'. If 'addr' lies
-   within the video memory region, we call video.c:vga_write(). */
+   within the video memory region, we call 'video.c:vga_write()'. */
 static void
 write_byte(u_int32_t addr, u_int8_t val)
 {
-    if (addr >= 0xa0000 && addr < 0xb0000) {
+    if (addr >= 0xa0000 && addr < 0xb0000)
 	vga_write(addr, val);
-    } else
+    else
 	*(u_int8_t *)addr = val;
 
     return;
 }
 
 /* Write a 16-bit value to the location specified by 'addr'. If 'addr' lies
-   within the video memory region, we call video.c:vga_write(). */
+   within the video memory region, we call 'video.c:vga_write()'. */
 static void
 write_word(u_int32_t addr, u_int16_t val)
 {
diff --git a/usr.bin/doscmd/tty.c b/usr.bin/doscmd/tty.c
index f6b9449..ceab613 100644
--- a/usr.bin/doscmd/tty.c
+++ b/usr.bin/doscmd/tty.c
@@ -115,7 +115,7 @@ typedef struct TextLine {
     u_char	max_length;	/* Not used, but here for future use */
     u_char	changed:1;
 } TextLine;
-TextLine *lines;
+TextLine *lines = NULL;
 
 int kbd_fd = -1;
 int kbd_read = 0;
@@ -133,14 +133,14 @@ SetVREGCur()
     VGA_CRTC[CRTC_CurLocLo] = cp & 0xff;
 }
 
-void	_kbd_event(void *);
-void	debug_event(void *);
-int	video_event();
-void	video_async_event(void *);
-void	tty_cooked();
-unsigned char inb_port60(int);
-void	kbd_event(int fd, REGISTERS);
-u_short	read_raw_kbd(int fd, u_short *code);
+void		_kbd_event(void *);
+void		debug_event(void *);
+int		video_event();
+void		video_async_event(void *);
+void		tty_cooked();
+unsigned char	inb_port60(int);
+void		kbd_event(int);
+u_short		read_raw_kbd(int, u_short *);
 
 /* Local functions */
 #ifndef NO_X
@@ -364,7 +364,7 @@ video_update(regcontext_t *REGS)
 	static int icnt = 3;
 
 	if (kbd_read)
-	    kbd_event(kbd_fd, REGS);
+	    kbd_event(kbd_fd);
 
     	if (--icnt == 0) {
 	    icnt = 3;
@@ -791,7 +791,7 @@ inb_port60(int port)
 }       
 
 void
-kbd_event(int fd, REGISTERS)
+kbd_event(int fd)
 {
     kbd_read = 0;
 
@@ -998,7 +998,7 @@ video_async_event(void *pfd)
                 int x;
                 fd_set fdset;
                 XEvent ev;  
-                static struct timeval tv = { 0 };
+                static struct timeval tv;
  
                 /*
                  * Handle any events just sitting around...
@@ -1042,7 +1042,7 @@ video_async_event(void *pfd)
 }
 
 void
-kbd_async_event(int fd, REGISTERS)
+kbd_async_event(int fd)
 {
     unsigned char c;
 
@@ -2182,7 +2182,7 @@ get_ximage()
         XDestroyImage(xi);
         err(1, "Could not get memory for ximage data");
     }
-    
+
     return;
 #endif
 }
@@ -2193,14 +2193,31 @@ get_lines()
 {
     int i;
     
-    if (!(lines = (TextLine *)malloc(sizeof(TextLine) * height)))
-	err(1, "Could not allocate data structure for text lines\n");
+    if (lines == NULL) {
+	lines = (TextLine *)malloc(sizeof(TextLine) * height);
+	if (lines == NULL)
+	    err(1, "Could not allocate data structure for text lines\n");
 
-    for (i = 0; i < height; ++i) {
-	lines[i].max_length = width;
-	if (!(lines[i].data = (u_short *)malloc(width * sizeof(u_short))))
+	for (i = 0; i < height; ++i) {
+	    lines[i].max_length = width;
+	    lines[i].data = (u_short *)malloc(width * sizeof(u_short));
+	    if (lines[i].data == NULL)
+		err(1, "Could not allocate data structure for text lines\n");
+	    lines[i].changed = 1;
+	}
+    } else {
+	lines = (TextLine *)realloc(lines, sizeof(TextLine) * height);
+	if (lines == NULL)
 	    err(1, "Could not allocate data structure for text lines\n");
-	lines[i].changed = 1;
+
+	for (i = 0; i < height; ++i) {
+	    lines[i].max_length = width;
+	    lines[i].data = (u_short *)realloc(lines[i].data,
+					       width * sizeof(u_short));
+	    if (lines[i].data == NULL)
+		err(1, "Could not allocate data structure for text lines\n");
+	    lines[i].changed = 1;
+	}
     }
 }
 
@@ -2258,7 +2275,8 @@ resize_window()
     sh->min_height = sh->max_height = sh->base_height;
     sh->flags = USSize | PMinSize | PMaxSize | PSize;
 
-    debug(D_VIDEO, "window size %dx%d\n", sh->base_width, sh->base_height);
+    debug(D_VIDEO, "VGA: Set window size %dx%d\n",
+	  sh->base_width, sh->base_height);
     
     XSetWMNormalHints(dpy, win, sh);
     XResizeWindow(dpy, win, sh->base_width, sh->base_height);
diff --git a/usr.bin/doscmd/video.c b/usr.bin/doscmd/video.c
index 6c5fabc3..3189225 100644
--- a/usr.bin/doscmd/video.c
+++ b/usr.bin/doscmd/video.c
@@ -175,7 +175,7 @@ video_outb(int port, u_int8_t value)
 	    col = cp % DpyCols;
 	    break;
 	default:
-	    debug(D_VIDEO, "outb 0x%04x, 0x%02x at index 0x%02x\n",
+	    debug(D_VIDEO, "VGA: outb 0x%04x, 0x%02x at index 0x%02x\n",
 		  port, value, crtc_index);
 	    break;
 	}
@@ -190,7 +190,7 @@ video_outb(int port, u_int8_t value)
 	    VGA_ATC[atc_index] = value;
 	    switch (atc_index) {
 	    default:
-		debug(D_VIDEO, "outb 0x%04x, 0x%02x at index 0x%02x\n",
+		debug(D_VIDEO, "VGA: outb 0x%04x, 0x%02x at index 0x%02x\n",
 		      port, value, crtc_index);
 		break;
 	    }
@@ -204,7 +204,7 @@ video_outb(int port, u_int8_t value)
 	VGA_TSC[tsc_index] = value;
 	switch (tsc_index) {
 	default:
-	    debug(D_VIDEO, "outb 0x%04x, 0x%02x at index 0x%02x\n",
+	    debug(D_VIDEO, "VGA: outb 0x%04x, 0x%02x at index 0x%02x\n",
 		  port, value, crtc_index);
 	    break;
 	}
@@ -217,7 +217,7 @@ video_outb(int port, u_int8_t value)
 #if 0
 	switch (gdc_index) {
 	default:
-	    debug(D_VIDEO, "outb 0x%04x, 0x%02x at index 0x%02x\n",
+	    debug(D_VIDEO, "VGA: outb 0x%04x, 0x%02x at index 0x%02x\n",
 		  port, value, crtc_index);
 
 	    break;
@@ -225,7 +225,7 @@ video_outb(int port, u_int8_t value)
 #endif
 	break;
     default:
-	debug(D_ALWAYS, "Unknown port 0x%4x\n", port);
+	debug(D_ALWAYS, "VGA: Unknown port 0x%4x\n", port);
 	break;
     }
 	
@@ -385,7 +385,7 @@ init_mode(int mode)
     int idx;			/* Index into vmode */
     int pidx;			/* Index into videoparams */
     
-    debug(D_VIDEO, "Set video mode to 0x%02x\n", mode);
+    debug(D_VIDEO, "VGA: Set video mode to 0x%02x\n", mode);
 
     idx = find_vmode(mode & 0x7f);
     if (idx == -1 || vmodelist[idx].type == NOMODE)
@@ -466,7 +466,7 @@ init_mode(int mode)
        0xaffff will generate a T_PAGEFAULT trap in VM86 mode (aside: why not a
        SIGSEGV?), which is handled in 'trap.c:sigbus()'. */
     if (vmode.type == GRAPHICS) {
-	vmem = mmap((void *)0xa0000, 64 * 1024, PROT_READ,
+	vmem = mmap((void *)0xa0000, 64 * 1024, PROT_NONE,
 		    MAP_ANON | MAP_FIXED | MAP_INHERIT | MAP_SHARED, -1, 0);
 	if (vmem == NULL)
 	    fatal("Could not mmap() video memory");
@@ -493,7 +493,7 @@ init_mode(int mode)
    'videoparams' array. */
 int find_vmode(int mode)
 {
-    int i;
+    unsigned i;
 
     for (i = 0; i < NUMMODES; i++)
 	if (vmodelist[i].modenumber == mode)
@@ -502,8 +502,6 @@ int find_vmode(int mode)
     return -1;
 }
 
-
-
 /* Handle access to the graphics memory.
 
    Simply changing the protection for the memory is not enough, unfortunately.
@@ -534,6 +532,35 @@ vmem_pageflt(struct sigframe *sf)
     return emu_instr(REGS);
 }
 
+/* We need to keep track of the latches' contents.*/
+static u_int8_t latch0, latch1, latch2, latch3;
+
+/* Read a byte from the video memory. 'vga_read()' is called from
+   'cpu.c:read_byte()' and will emulate the VGA read modes. */
+u_int8_t
+vga_read(u_int32_t addr)
+{
+    u_int32_t dst;
+    
+    /* 'addr' lies between 0xa0000 and 0xaffff. */
+    dst = addr - 0xa0000;
+
+    /* Fill latches. */
+    latch0 = vplane0[dst];
+    latch1 = vplane1[dst];
+    latch2 = vplane2[dst];
+    latch3 = vplane3[dst];
+    
+    /* Select read mode. */
+    if ((VGA_GDC[GDC_Mode] & 0x80) == 0)
+	/* Read Mode 0; return the byte from the selected bit plane. */
+	return vram[dst + (VGA_GDC[GDC_ReadMapSelect] & 3) * 0x10000];
+
+    /* Read Mode 1 */
+    debug(D_ALWAYS, "VGA: Read Mode 1 not implemented\n");
+    return 0;
+}
+
 /* Write a byte to the video memory. 'vga_write()' is called from
    'cpu.c:write_word()' and will emulate the VGA write modes. Not all four
    modes are implemented yet, nor are the addressing modes (odd/even, chain4).
@@ -543,46 +570,41 @@ void
 vga_write(u_int32_t addr, u_int8_t val)
 {
     u_int32_t dst;
-    u_int8_t *latch0, *latch1, *latch2, *latch3;
     u_int8_t c0, c1, c2, c3;
     u_int8_t m0, m1, m2, m3;
     u_int8_t mask;
+
 #if 0
-    int i;
+    unsigned i;
     
-    debug(D_VIDEO, "Write 0x%02x to 0x%x\n", val, addr);
-    debug(D_VIDEO, "GDC: ");
+    debug(D_VIDEO, "VGA: Write 0x%02x to 0x%x\n", val, addr);
+    debug(D_VIDEO, "   GDC: ");
     for (i = 0; i < sizeof(VGA_GDC); i++)
 	debug(D_VIDEO, "%02x ", VGA_GDC[i]);
     debug(D_VIDEO, "\n");
-    debug(D_VIDEO, "TSC: ");
+    debug(D_VIDEO, "   TSC: ");
     for (i = 0; i < sizeof(VGA_TSC); i++)
 	debug(D_VIDEO, "%02x ", VGA_TSC[i]);
     debug(D_VIDEO, "\n");
 #endif
     
-    /* 'addr' lies between 0xa0000 and 0xaffff */
+    /* 'addr' lies between 0xa0000 and 0xaffff. */
     dst = addr - 0xa0000;
 
-    /* fill latches */
-    latch0 = vplane0 + dst;
-    latch1 = vplane1 + dst;
-    latch2 = vplane2 + dst;
-    latch3 = vplane3 + dst;
-
-    c0 = *latch0;
-    c1 = *latch1;
-    c2 = *latch2;
-    c3 = *latch3;
+    c0 = latch0;
+    c1 = latch1;
+    c2 = latch2;
+    c3 = latch3;
     
-    /* select write mode */
+    /* Select write mode. */
     switch (VGA_GDC[GDC_Mode] & 3) {
     case 0:
-	/* XXX to do: Enable Set Reset register */
-	
 	mask = VGA_GDC[GDC_BitMask];
 
-	/* select function */
+	if (VGA_GDC[GDC_DataRotate] & 7)
+	    debug(D_ALWAYS, "VGA: Data Rotate != 0\n");
+	
+	/* Select function.  */
 	switch (VGA_GDC[GDC_DataRotate] & 0x18) {
 	case 0x00:		/* replace */
 	    m0 = VGA_GDC[GDC_SetReset] & 1 ? mask : 0x00;
@@ -590,53 +612,54 @@ vga_write(u_int32_t addr, u_int8_t val)
 	    m2 = VGA_GDC[GDC_SetReset] & 4 ? mask : 0x00;
 	    m3 = VGA_GDC[GDC_SetReset] & 8 ? mask : 0x00;
 
-	    c0 &= ~mask;
-	    c1 &= ~mask;
-	    c2 &= ~mask;
-	    c3 &= ~mask;
+	    c0 = VGA_GDC[GDC_EnableSetReset] & 1 ? c0 & ~mask : val & ~mask;
+	    c1 = VGA_GDC[GDC_EnableSetReset] & 2 ? c1 & ~mask : val & ~mask;
+	    c2 = VGA_GDC[GDC_EnableSetReset] & 4 ? c2 & ~mask : val & ~mask;
+	    c3 = VGA_GDC[GDC_EnableSetReset] & 8 ? c3 & ~mask : val & ~mask;
     
 	    c0 |= m0;
 	    c1 |= m1;
 	    c2 |= m2;
 	    c3 |= m3;
 	    break;
-	case 0x08:		/* and */
+	case 0x08:		/* AND */
 	    m0 = VGA_GDC[GDC_SetReset] & 1 ? 0xff : ~mask;
 	    m1 = VGA_GDC[GDC_SetReset] & 2 ? 0xff : ~mask;
 	    m2 = VGA_GDC[GDC_SetReset] & 4 ? 0xff : ~mask;
 	    m3 = VGA_GDC[GDC_SetReset] & 8 ? 0xff : ~mask;
 
-	    c0 &= m0;
-	    c1 &= m1;
-	    c2 &= m2;
-	    c3 &= m3;
+	    c0 = VGA_GDC[GDC_EnableSetReset] & 1 ? c0 & m0 : val & m0;
+	    c1 = VGA_GDC[GDC_EnableSetReset] & 2 ? c1 & m1 : val & m1;
+	    c2 = VGA_GDC[GDC_EnableSetReset] & 4 ? c2 & m2 : val & m2;
+	    c3 = VGA_GDC[GDC_EnableSetReset] & 8 ? c3 & m3 : val & m3;
 	    break;
-	case 0x10:		/* or */
+	case 0x10:		/* OR */
 	    m0 = VGA_GDC[GDC_SetReset] & 1 ? mask : 0x00;
 	    m1 = VGA_GDC[GDC_SetReset] & 2 ? mask : 0x00;
 	    m2 = VGA_GDC[GDC_SetReset] & 4 ? mask : 0x00;
 	    m3 = VGA_GDC[GDC_SetReset] & 8 ? mask : 0x00;
 
-	    c0 |= m0;
-	    c1 |= m1;
-	    c2 |= m2;
-	    c3 |= m3;
+	    c0 = VGA_GDC[GDC_EnableSetReset] & 1 ? c0 | m0 : val | m0;
+	    c1 = VGA_GDC[GDC_EnableSetReset] & 2 ? c1 | m1 : val | m1;
+	    c2 = VGA_GDC[GDC_EnableSetReset] & 4 ? c2 | m2 : val | m2;
+	    c3 = VGA_GDC[GDC_EnableSetReset] & 8 ? c3 | m3 : val | m3;
 	    break;
-	case 0x18:		/* xor */
+	case 0x18:		/* XOR */
 	    m0 = VGA_GDC[GDC_SetReset] & 1 ? mask : 0x00;
 	    m1 = VGA_GDC[GDC_SetReset] & 2 ? mask : 0x00;
 	    m2 = VGA_GDC[GDC_SetReset] & 4 ? mask : 0x00;
 	    m3 = VGA_GDC[GDC_SetReset] & 8 ? mask : 0x00;
 
-	    c0 ^= m0;
-	    c1 ^= m1;
-	    c2 ^= m2;
-	    c3 ^= m3;
+	    c0 = VGA_GDC[GDC_EnableSetReset] & 1 ? c0 ^ m0 : val ^ m0;
+	    c1 = VGA_GDC[GDC_EnableSetReset] & 2 ? c1 ^ m1 : val ^ m1;
+	    c2 = VGA_GDC[GDC_EnableSetReset] & 4 ? c2 ^ m2 : val ^ m2;
+	    c3 = VGA_GDC[GDC_EnableSetReset] & 8 ? c3 ^ m3 : val ^ m3;
 	    break;
 	}
 	break;
     case 1:
-	/* not yet */
+	/* Just copy the latches' content to the desired destination
+	   address. */
 	break;
     case 2:
 	mask = VGA_GDC[GDC_BitMask];
@@ -696,18 +719,19 @@ vga_write(u_int32_t addr, u_int8_t val)
 	break;
     case 3:
 	/* not yet */
+	debug(D_ALWAYS, "VGA: Write Mode 3 not implemented\n");
 	break;
     }
 
-    /* write back changed byte, depending on Map Mask register */
+    /* Write back changed byte, depending on Map Mask register. */
     if (VGA_TSC[TSC_MapMask] & 1)
-	*latch0 = c0;
+	vplane0[dst] = c0;
     if (VGA_TSC[TSC_MapMask] & 2)
-	*latch1 = c1;
+	vplane1[dst] = c1;
     if (VGA_TSC[TSC_MapMask] & 4)
-	*latch2 = c2;
+	vplane2[dst] = c2;
     if (VGA_TSC[TSC_MapMask] & 8)
-	*latch3 = c3;
+	vplane3[dst] = c3;
     
     return;
 }
diff --git a/usr.bin/doscmd/video.h b/usr.bin/doscmd/video.h
index 32daebb..e765d2a 100644
--- a/usr.bin/doscmd/video.h
+++ b/usr.bin/doscmd/video.h
@@ -364,13 +364,14 @@ extern vmode_t vmodelist[];
 #define FONT8x16	"vga8x16"	/* same as FONTVGA */
 
 /* External functions in 'video.c'. */
-extern void	init_mode(int);
-extern int	find_vmode(int);
-extern void	vga_write(u_int32_t, u_int8_t);
-extern void	video_bios_init(void);
-extern void	video_init(void);
-extern int	vmem_pageflt(struct sigframe *);
+void		init_mode(int);
+int		find_vmode(int);
+u_int8_t	vga_read(u_int32_t);
+void		vga_write(u_int32_t, u_int8_t);
+void		video_bios_init(void);
+void		video_init(void);
+int		vmem_pageflt(struct sigframe *);
 
 /* Other external variables, mostly from tty.c. Needs to be cleaned up. */
-extern int vattr;
-void	write_vram(void *);
+extern int 	vattr;
+void		write_vram(void *);