Kernel virtual mode 8086, currently disabled in GENERIC.

lots of help at c2k6 and after from toby@,
ok toby@, mickey@

6 files changed, 610 insertions, 6 deletions

diff --git a/sys/arch/i386/conf/GENERIC b/sys/arch/i386/conf/GENERIC
index e9725b5bcaa..eda3cab202b 100644
--- a/sys/arch/i386/conf/GENERIC
+++ b/sys/arch/i386/conf/GENERIC
@@ -1,4 +1,4 @@
-#	$OpenBSD: GENERIC,v 1.504 2006/06/07 18:47:16 deraadt Exp $
+#	$OpenBSD: GENERIC,v 1.505 2006/06/13 03:01:04 gwk Exp $
 #
 # For further information on compiling OpenBSD kernels, see the config(8)
 # man page.
@@ -22,6 +22,7 @@ option		I686_CPU
 option		USER_PCICONF	# user-space PCI configuration
 
 #option		VM86		# Virtual 8086 emulation
+#option		KVM86		# Kernel Virtual 8086 emulation
 option		USER_LDT	# user-settable LDT; see i386_set_ldt(2)
 option		APERTURE	# in-kernel aperture driver for XFree86
 
diff --git a/sys/arch/i386/i386/autoconf.c b/sys/arch/i386/i386/autoconf.c
index a8000829ae1..cef6ce0ca4b 100644
--- a/sys/arch/i386/i386/autoconf.c
+++ b/sys/arch/i386/i386/autoconf.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: autoconf.c,v 1.62 2006/05/28 14:58:12 deraadt Exp $	*/
+/*	$OpenBSD: autoconf.c,v 1.63 2006/06/13 03:01:04 gwk Exp $	*/
 /*	$NetBSD: autoconf.c,v 1.20 1996/05/03 19:41:56 christos Exp $	*/
 
 /*-
@@ -116,6 +116,9 @@ cpu_configure()
 
 	gdt_init();		/* XXX - pcibios uses gdt stuff */
 
+	/* Set up proc0's TSS and LDT */
+	i386_proc0_tss_ldt_init();
+
 #ifndef SMALL_KERNEL
 	pmap_bootstrap_pae();
 #endif
@@ -129,6 +132,8 @@ cpu_configure()
 	ioapic_enable();
 #endif
 
+	proc0.p_addr->u_pcb.pcb_cr0 = rcr0();
+
 #ifdef MULTIPROCESSOR
 	/* propagate TSS and LDT configuration to the idle pcb's. */
 	cpu_init_idle_pcbs();
@@ -142,9 +147,6 @@ cpu_configure()
 	md_diskconf = diskconf;
 	cold = 0;
 
-	/* Set up proc0's TSS and LDT (after the FPU is configured). */
-	i386_proc0_tss_ldt_init();
-
 #ifdef I686_CPU
 	/*
 	 * At this point the RNG is running, and if FSXR is set we can
diff --git a/sys/arch/i386/i386/kvm86.c b/sys/arch/i386/i386/kvm86.c
new file mode 100644
index 00000000000..7cf1df582a0
--- /dev/null
+++ b/sys/arch/i386/i386/kvm86.c
@@ -0,0 +1,348 @@
+/* $OpenBSD: kvm86.c,v 1.1 2006/06/13 03:01:04 gwk Exp $ */
+/* $NetBSD: kvm86.c,v 1.10 2005/12/26 19:23:59 perry Exp $ */
+/*
+ * Copyright (c) 2002
+ * 	Matthias Drochner.  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
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT 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.
+ */
+#include <sys/cdefs.h>
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/proc.h>
+#include <sys/user.h>
+#include <sys/malloc.h>
+#include <uvm/uvm_extern.h>
+#include <uvm/uvm.h>
+#include <machine/pcb.h>
+#include <machine/pte.h>
+#include <machine/pmap.h>
+#include <machine/kvm86.h>
+#include <machine/cpu.h>
+
+/* assembler functions in kvm86call.s */
+extern int kvm86_call(struct trapframe *);
+extern void kvm86_ret(struct trapframe *, int);
+
+#define PGTABLE_SIZE	((1024 + 64) * 1024 / PAGE_SIZE)
+
+/* XXX: not here */
+typedef u_int32_t pt_entry_t;
+
+struct kvm86_data {
+	pt_entry_t pgtbl[PGTABLE_SIZE];
+
+	struct segment_descriptor sd;
+
+	struct pcb pcb;
+	u_long iomap[0x10000/32];
+};
+
+void kvm86_map(struct kvm86_data *, paddr_t, uint32_t);
+void kvm86_mapbios(struct kvm86_data *);
+void kvm86_prepare(struct kvm86_data *vmd);
+/*
+ * global VM for BIOS calls
+ */
+struct kvm86_data *bioscallvmd;
+/* page for trampoline and stack */
+void *bioscallscratchpage;
+/* where this page is mapped in the vm86 */
+#define BIOSCALLSCRATCHPAGE_VMVA 0x1000
+/* a virtual page to map in vm86 memory temporarily */
+vaddr_t bioscalltmpva;
+
+#define KVM86_IOPL3 /* not strictly necessary, saves a lot of traps */
+
+void
+kvm86_init()
+{
+	size_t vmdsize;
+	char *buf;
+	struct kvm86_data *vmd;
+	struct pcb *pcb;
+	paddr_t pa;
+	int i;
+
+	vmdsize = round_page(sizeof(struct kvm86_data)) + PAGE_SIZE;
+
+	if ((buf = (char *)uvm_km_zalloc(kernel_map, vmdsize)) == NULL)
+		return;
+	
+	/* first page is stack */
+	vmd = (struct kvm86_data *)(buf + PAGE_SIZE);
+	pcb = &vmd->pcb;
+
+	/*
+	 * derive pcb and TSS from proc0
+	 * we want to access all IO ports, so we need a full-size
+	 *  permission bitmap
+	 * XXX do we really need the pcb or just the TSS?
+	 */
+	memcpy(pcb, &proc0.p_addr->u_pcb, sizeof(struct pcb));
+	pcb->pcb_tss.tss_esp0 = (int)vmd;
+	pcb->pcb_tss.tss_ss0 = GSEL(GDATA_SEL, SEL_KPL);
+	for (i = 0; i < sizeof(vmd->iomap) / 4; i++)
+		vmd->iomap[i] = 0;
+	pcb->pcb_tss.tss_ioopt =
+	    ((caddr_t)vmd->iomap - (caddr_t)&pcb->pcb_tss) << 16;
+
+	/* setup TSS descriptor (including our iomap) */
+	setsegment(&vmd->sd, &pcb->pcb_tss,
+	    sizeof(struct pcb) + sizeof(vmd->iomap) - 1,
+	    SDT_SYS386TSS, SEL_KPL, 0, 0);
+
+	/* prepare VM for BIOS calls */
+	kvm86_mapbios(vmd);
+	if ((bioscallscratchpage = (void *)uvm_km_alloc(kernel_map, PAGE_SIZE))
+	    == 0)
+		return;
+
+	pmap_extract(pmap_kernel(), (vaddr_t)bioscallscratchpage, &pa);
+	kvm86_map(vmd, pa, BIOSCALLSCRATCHPAGE_VMVA);
+	bioscallvmd = vmd;
+	bioscalltmpva = uvm_km_alloc(kernel_map, PAGE_SIZE);
+}
+
+/*
+ * XXX pass some stuff to the assembler code
+ * XXX this should be done cleanly (in call argument to kvm86_call())
+ */
+
+volatile struct pcb *vm86pcb;
+volatile int vm86tssd0, vm86tssd1;
+volatile paddr_t vm86newptd;
+volatile struct trapframe *vm86frame;
+volatile pt_entry_t *vm86pgtableva;
+
+void
+kvm86_prepare(struct kvm86_data *vmd)
+{
+	vm86newptd = vtophys((vaddr_t)vmd) | PG_V | PG_RW | PG_U | PG_u;
+	vm86pgtableva = vmd->pgtbl;
+	vm86frame = (struct trapframe *)vmd - 1;
+	vm86pcb = &vmd->pcb;
+	vm86tssd0 = *(int*)&vmd->sd;
+	vm86tssd1 = *((int*)&vmd->sd + 1);
+}
+
+void
+kvm86_map(struct kvm86_data *vmd, paddr_t pa, uint32_t vmva)
+{
+
+	vmd->pgtbl[vmva >> 12] = pa | PG_V | PG_RW | PG_U | PG_u;
+}
+
+void
+kvm86_mapbios(struct kvm86_data *vmd)
+{
+	paddr_t pa;
+
+	/* map first physical page (vector table, BIOS data) */
+	kvm86_map(vmd, 0, 0);
+
+	/* map ISA hole */
+	for (pa = 0xa0000; pa < 0x100000; pa += PAGE_SIZE)
+		kvm86_map(vmd, pa, pa);
+}
+
+void *
+kvm86_bios_addpage(uint32_t vmva)
+{
+	void *mem;
+	paddr_t pa;
+
+	if (bioscallvmd->pgtbl[vmva >> 12]) /* allocated? */
+		return (NULL);
+
+	if ((mem = (void *)uvm_km_alloc(kernel_map, PAGE_SIZE)) == NULL)
+		return (NULL);
+	
+	pmap_extract(pmap_kernel(), (vaddr_t)mem, &pa);	
+	kvm86_map(bioscallvmd, pa, vmva);
+
+	return (mem);
+}
+
+void
+kvm86_bios_delpage(uint32_t vmva, void *kva)
+{
+
+	bioscallvmd->pgtbl[vmva >> 12] = 0;
+	uvm_km_free(kernel_map, (vaddr_t)kva, PAGE_SIZE);
+}
+
+size_t
+kvm86_bios_read(u_int32_t vmva, char *buf, size_t len)
+{
+	size_t todo, now;
+	paddr_t vmpa;
+
+	todo = len;
+	while (todo > 0) {
+		now = min(todo, PAGE_SIZE - (vmva & (PAGE_SIZE - 1)));
+
+		if (!bioscallvmd->pgtbl[vmva >> 12])
+			break;
+		vmpa = bioscallvmd->pgtbl[vmva >> 12] & ~(PAGE_SIZE - 1);
+		pmap_kenter_pa(bioscalltmpva, vmpa, VM_PROT_READ);
+		pmap_update(pmap_kernel());
+
+		memcpy(buf, (void *)(bioscalltmpva + (vmva & (PAGE_SIZE - 1))),
+		       now);
+		buf += now;
+		todo -= now;
+		vmva += now;
+	}
+	return (len - todo);
+}
+
+int
+kvm86_bioscall(int intno, struct trapframe *tf)
+{
+	static const unsigned char call[] = {
+		0xfa, /* CLI */
+		0xcd, /* INTxx */
+		0,
+		0xfb, /* STI */
+		0xf4  /* HLT */
+	};
+
+	memcpy(bioscallscratchpage, call, sizeof(call));
+	*((unsigned char *)bioscallscratchpage + 2) = intno;
+
+	tf->tf_eip = BIOSCALLSCRATCHPAGE_VMVA;
+	tf->tf_cs = 0;
+	tf->tf_esp = BIOSCALLSCRATCHPAGE_VMVA + PAGE_SIZE - 2;
+	tf->tf_ss = 0;
+	tf->tf_eflags = PSL_USERSET | PSL_VM;
+#ifdef KVM86_IOPL3
+	tf->tf_eflags |= PSL_IOPL;
+#endif
+	tf->tf_ds = tf->tf_es = tf->tf_fs = tf->tf_gs = 0;
+
+	kvm86_prepare(bioscallvmd); /* XXX */
+	return (kvm86_call(tf));
+}
+
+int
+kvm86_simplecall(int no, struct kvm86regs *regs)
+{
+	struct trapframe tf;
+	int res;
+	
+	memset(&tf, 0, sizeof(struct trapframe));
+	tf.tf_eax = regs->eax;
+	tf.tf_ebx = regs->ebx;
+	tf.tf_ecx = regs->ecx;
+	tf.tf_edx = regs->edx;
+	tf.tf_esi = regs->esi;
+	tf.tf_edi = regs->edi;
+	tf.tf_vm86_es = regs->es;
+	
+	res = kvm86_bioscall(no, &tf);
+	
+	regs->eax = tf.tf_eax;
+	regs->ebx = tf.tf_ebx;
+	regs->ecx = tf.tf_ecx;
+	regs->edx = tf.tf_edx;
+	regs->esi = tf.tf_esi;
+	regs->edi = tf.tf_edi;
+	regs->es = tf.tf_vm86_es;
+	regs->eflags = tf.tf_eflags;
+	
+	return (res);
+}
+
+void
+kvm86_gpfault(struct trapframe *tf)
+{
+	unsigned char *kva, insn, trapno;
+	uint16_t *sp;
+
+	kva = (unsigned char *)((tf->tf_cs << 4) + tf->tf_eip);
+	insn = *kva;
+#ifdef KVM86DEBUG
+	printf("kvm86_gpfault: cs=%x, eip=%x, insn=%x, eflags=%x\n",
+	       tf->tf_cs, tf->tf_eip, insn, tf->tf_eflags);
+#endif
+
+	KASSERT(tf->tf_eflags & PSL_VM);
+
+	switch (insn) {
+	case 0xf4: /* HLT - normal exit */
+		kvm86_ret(tf, 0);
+		break;
+	case 0xcd: /* INTxx */
+		/* fake a return stack frame and call real mode handler */
+		trapno = *(kva + 1);
+		sp = (uint16_t *)((tf->tf_ss << 4) + tf->tf_esp);
+		*(--sp) = tf->tf_eflags;
+		*(--sp) = tf->tf_cs;
+		*(--sp) = tf->tf_eip + 2;
+		tf->tf_esp -= 6;
+		tf->tf_cs = *(uint16_t *)(trapno * 4 + 2);
+		tf->tf_eip = *(uint16_t *)(trapno * 4);
+		break;
+	case 0xcf: /* IRET */
+		sp = (uint16_t *)((tf->tf_ss << 4) + tf->tf_esp);
+		tf->tf_eip = *(sp++);
+		tf->tf_cs = *(sp++);
+		tf->tf_eflags = *(sp++);
+		tf->tf_esp += 6;
+		tf->tf_eflags |= PSL_VM; /* outside of 16bit flag reg */
+		break;
+#ifndef KVM86_IOPL3 /* XXX check VME? */
+	case 0xfa: /* CLI */
+	case 0xfb: /* STI */
+		/* XXX ignore for now */
+		tf->tf_eip++;
+		break;
+	case 0x9c: /* PUSHF */
+		sp = (uint16_t *)((tf->tf_ss << 4) + tf->tf_esp);
+		*(--sp) = tf->tf_eflags;
+		tf->tf_esp -= 2;
+		tf->tf_eip++;
+		break;
+	case 0x9d: /* POPF */
+		sp = (uint16_t *)((tf->tf_ss << 4) + tf->tf_esp);
+		tf->tf_eflags = *(sp++);
+		tf->tf_esp += 2;
+		tf->tf_eip++;
+		tf->tf_eflags |= PSL_VM; /* outside of 16bit flag reg */
+		break;
+#endif
+	default:
+#ifdef KVM86DEBUG
+		printf("kvm86_gpfault: unhandled\n");
+#else
+		printf("kvm86_gpfault: cs=%x, eip=%x, insn=%x, eflags=%x\n",
+		       tf->tf_cs, tf->tf_eip, insn, tf->tf_eflags);
+#endif
+		/*
+		 * signal error to caller
+		 */
+		kvm86_ret(tf, -1);
+		break;
+	}
+}
diff --git a/sys/arch/i386/i386/kvm86call.S b/sys/arch/i386/i386/kvm86call.S
new file mode 100644
index 00000000000..9f6ab4eb8f8
--- /dev/null
+++ b/sys/arch/i386/i386/kvm86call.S
@@ -0,0 +1,200 @@
+/* $OpenBSD: kvm86call.S,v 1.1 2006/06/13 03:01:04 gwk Exp $ */
+/* $NetBSD: kvm86call.S,v 1.7 2006/04/11 17:14:07 drochner Exp $ */
+
+/*-
+ * Copyright (c) 1998 Jonathan Lemon
+ * All rights reserved.
+ * Copyright (c) 2002
+ *	Matthias Drochner. 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
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT 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.
+ *
+ * $FreeBSD: src/sys/i386/i386/vm86bios.s,v 1.28 2001/12/11 23:33:40 jhb Exp $
+ */
+
+#include <machine/asm.h>
+#include <machine/param.h>
+#include "assym.h"
+
+	.data
+	.align 4
+
+/* flag for trap() */
+	.globl	kvm86_incall
+kvm86_incall:		.long	0
+
+/* XXX arguments for kvm86call() */
+	.globl vm86pcb, vm86newptd, vm86frame, vm86pgtableva
+	.globl	vm86tssd0, vm86tssd1
+
+/* locals */
+SCRARGFRAME:		.long	0
+SCRSTACK:		.long	0
+SCRTSS0:		.long	0
+SCRTSS1:		.long	0
+
+	.text
+
+#if defined MULTIPROCESSOR
+#include <machine/i82489reg.h>
+
+#define GET_CPUINFO(reg)						\
+	movl	_C_LABEL(lapic_id),reg ;				\
+	shrl	$LAPIC_ID_SHIFT,reg	;				\
+	movl	_C_LABEL(cpu_info),reg  
+#else
+#define	GET_CPUINFO(reg) 						\
+	leal	_C_LABEL(cpu_info_primary),reg
+#endif
+
+#define GET_CURPCB(reg)							\
+	GET_CPUINFO(reg) ;						\
+	movl	CPU_INFO_CURPCB(reg),reg	
+
+#define SET_CURPCB(reg, treg)						\
+	GET_CPUINFO(treg) ;						\
+	movl	reg,CPU_INFO_CURPCB(treg)
+
+/* int kvm86_call(struct trapframe *) */
+ENTRY(kvm86_call)
+	pushl	%ebp
+	movl	%esp,%ebp		/* set up frame ptr */
+	pushl	%esi
+	pushl	%edi
+	pushl	%ebx
+	pushl	%fs
+	pushl	%gs
+
+	movl	8(%ebp),%eax
+	movl	%eax,SCRARGFRAME	/* save argument pointer */
+
+	movl	vm86frame,%edi		/* target frame location */
+	movl    SCRARGFRAME,%esi	/* source (set on entry) */
+	movl	$FRAMESIZE/4,%ecx	/* sizeof(struct trapframe)/4 */
+	cld
+	rep
+	movsl				/* copy frame to new stack */
+	GET_CURPCB(%eax)
+	pushl	%eax	
+	movl	vm86pcb,%eax
+	SET_CURPCB(%eax,%ecx)	/* set curpcb to vm86pcb */
+	movl	CPU_INFO_CURPROC(%ecx),%ecx
+	movl	P_MD_TSS_SEL(%ecx),%edi
+#ifdef	MULTIPROCESSOR
+	movl	CPU_INFO_GDT(%ecx),%eax
+#else
+	movl	_C_LABEL(gdt),%eax
+#endif
+	andl	$~0x0200,4(%eax,%edi,1)	/* reset "task busy" */
+
+	movl	0(%eax,%edi,1),%edx
+	movl	%edx,SCRTSS0		/* save first word */
+	movl	4(%eax,%edi,1),%edx
+	movl	%edx,SCRTSS1		/* save second word */
+
+	movl	vm86tssd0,%edx		/* vm86 tssd entry */
+	movl	%edx,0(%eax,%edi,1)
+	movl	vm86tssd1,%edx		/* vm86 tssd entry */
+	movl	%edx,4(%eax,%edi,1)
+	ltr	%di
+
+	movl	%cr3,%eax
+	pushl	%eax			/* save address space */
+	movl	PTDpaddr,%ecx
+	movl	%ecx,%ebx
+	addl	$KERNBASE,%ebx		/* va of Idle PDP */
+	movl	0(%ebx),%eax
+	pushl	%eax			/* old pde */
+	pushl	%ebx			/* keep address for reuse */
+
+	movl	%esp,SCRSTACK		/* save current stack location */
+
+	movl	vm86newptd,%eax		/* mapping for vm86 page table */
+	movl	%eax,0(%ebx)		/* ... install as PDP entry 0 */
+
+	movl	%ecx,%cr3		/* new page tables */
+	movl	vm86frame,%esp		/* switch to new stack */
+
+	movl	$1,kvm86_incall		/* set flag for trap() */
+
+	/* INTRFASTEXIT */
+	popl	%gs
+	popl	%fs
+	popl	%es
+	popl	%ds
+	popl	%edi
+	popl	%esi
+	popl	%ebp
+	popl	%ebx
+	popl	%edx
+	popl	%ecx
+	popl	%eax
+	addl	$8,%esp
+	iret
+
+
+/* void kvm86_ret(struct trapframe *, int) */
+ENTRY(kvm86_ret)
+	pushl	%ebp
+	movl	%esp,%ebp		/* set up frame ptr */
+
+	movl	8(%ebp),%esi		/* source */
+	movl	SCRARGFRAME,%edi	/* destination */
+	movl	$FRAMESIZE/4,%ecx	/* size */
+	cld
+	rep
+	movsl				/* copy frame to original frame */
+
+	movl	SCRSTACK,%esp		/* back to old stack */
+	popl	%ebx			/* saved va of Idle PDP */
+	popl	%eax
+	movl	%eax,0(%ebx)		/* restore old pde */
+	popl	%eax
+	movl	%eax,%cr3		/* install old page table */
+
+	movl	$0,kvm86_incall		/* reset trapflag */
+	GET_CPUINFO(%ecx)
+#ifdef MULTIPROCESSOR
+	movl	CPU_INFO_GDT(%ecx),%eax
+#else
+	movl	_C_LABEL(gdt),%eax
+#endif
+	movl	CPU_INFO_CURPROC(%ecx),%ecx
+	movl	P_MD_TSS_SEL(%ecx),%edi
+	movl	SCRTSS0, %edx
+	movl	%edx, 0(%eax,%edi,1)	/* restore first word */
+	movl	SCRTSS1, %edx
+	movl	%edx, 4(%eax,%edi,1)	/* restore second word */
+	ltr	%di
+
+	popl	%eax			/* restore curpcb */
+	SET_CURPCB(%eax,%ecx)
+
+	movl	12(%ebp), %eax		/* will be kvm86_call()'s retval */
+
+	popl	%gs
+	popl	%fs
+	popl	%ebx
+	popl	%edi
+	popl	%esi
+	popl	%ebp
+	ret				/* back to kvm86_call()'s caller */
diff --git a/sys/arch/i386/i386/trap.c b/sys/arch/i386/i386/trap.c
index 330e6682b58..cd1570a67a3 100644
--- a/sys/arch/i386/i386/trap.c
+++ b/sys/arch/i386/i386/trap.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: trap.c,v 1.74 2006/05/11 13:21:12 mickey Exp $	*/
+/*	$OpenBSD: trap.c,v 1.75 2006/06/13 03:01:04 gwk Exp $	*/
 /*	$NetBSD: trap.c,v 1.95 1996/05/05 06:50:02 mycroft Exp $	*/
 
 /*-
@@ -90,6 +90,10 @@ extern struct emul emul_bsdos;
 #ifdef COMPAT_AOUT
 extern struct emul emul_aout;
 #endif
+#ifdef KVM86
+#include <machine/kvm86.h>
+#define KVM86MODE (kvm86_incall)
+#endif
 
 #include "npx.h"
 
@@ -272,6 +276,12 @@ trap(frame)
 		/*NOTREACHED*/
 
 	case T_PROTFLT:
+#ifdef KVM86
+		if (KVM86MODE) {
+			kvm86_gpfault(&frame);
+			return;
+		}
+#endif
 	case T_SEGNPFLT:
 	case T_ALIGNFLT:
 		/* Check for copyin/copyout fault. */
diff --git a/sys/arch/i386/include/kvm86.h b/sys/arch/i386/include/kvm86.h
new file mode 100644
index 00000000000..358208200fb
--- /dev/null
+++ b/sys/arch/i386/include/kvm86.h
@@ -0,0 +1,43 @@
+/* $OpenBSD: kvm86.h,v 1.1 2006/06/13 03:01:04 gwk Exp $ */
+/*
+ * Copyright (c) 2006 Gordon Willem Klok <gwk@openbsd.org>
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef _I386_KVM86_H_
+#define _I386_KVM86_H_
+
+struct kvm86regs {
+	uint32_t eax;
+	uint32_t ebx;
+	uint32_t ecx;
+	uint32_t edx;
+	uint32_t esi;
+	uint32_t edi;
+	uint32_t eflags;
+	uint32_t es;
+};
+
+extern int kvm86_incall;
+
+void kvm86_init(void);
+void kvm86_gpfault(struct trapframe *);
+
+void *kvm86_bios_addpage(uint32_t);
+void kvm86_bios_delpage(uint32_t, void *);
+size_t kvm86_bios_read(uint32_t, char *, size_t);
+
+int kvm86_bioscall(int, struct trapframe *);
+int kvm86_simplecall(int, struct kvm86regs *);
+
+#endif