Initial bhyve native graphics support.

This adds emulations for a raw framebuffer device, PS2 keyboard/mouse,
XHCI USB controller and a USB tablet.

A simple VNC server is provided for keyboard/mouse input, and graphics
output.

A VGA emulation is included, but is currently disconnected until an
additional bhyve change to block out VGA memory is committed.

Credits:
 - raw framebuffer, VNC server, XHCI controller, USB bus/device emulation
    and UEFI f/w support by Leon Dang
 - VGA, console/g, initial VNC server  by tychon@
 - PS2 keyboard/mouse jointly done by tychon@ and Leon Dang
 - hypervisor framebuffer mem support by neel@

Tested by: Michael Dexter, in a number of revisions of this code.

With the appropriate UEFI image, FreeBSD, Windows and Linux guests can
installed and run in graphics mode using the UEFI/GOP framebuffer.

24 files changed, 9035 insertions, 17 deletions

diff --git a/Makefile b/Makefile
index 7d2e2f0ed171..f010f3d408d6 100644
--- a/Makefile
+++ b/Makefile
@@ -14,9 +14,11 @@ BHYVE_SYSDIR?=${SRCTOP}
 SRCS=	\
 	atkbdc.c		\
 	acpi.c			\
+	bhyvegc.c		\
 	bhyverun.c		\
 	block_if.c		\
 	bootrom.c		\
+	console.c		\
 	consport.c		\
 	dbgport.c		\
 	fwctl.c			\
@@ -27,6 +29,7 @@ SRCS=	\
 	mptbl.c			\
 	pci_ahci.c		\
 	pci_emul.c		\
+	pci_fbuf.c		\
 	pci_hostbridge.c	\
 	pci_irq.c		\
 	pci_lpc.c		\
@@ -35,20 +38,30 @@ SRCS=	\
 	pci_virtio_net.c	\
 	pci_virtio_rnd.c	\
 	pci_uart.c		\
+	pci_xhci.c		\
 	pm.c			\
 	post.c			\
+	ps2kbd.c		\
+	ps2mouse.c		\
+	rfb.c			\
 	rtc.c			\
 	smbiostbl.c		\
+	sockstream.c		\
 	task_switch.c		\
 	uart_emul.c		\
+	usb_emul.c		\
+	usb_mouse.c		\
 	virtio.c		\
+	vga.c			\
 	xmsr.c			\
 	spinup_ap.c
 
 .PATH:  ${BHYVE_SYSDIR}/sys/amd64/vmm
 SRCS+=	vmm_instruction_emul.c
 
-LIBADD=	vmmapi md pthread
+LIBADD=	vmmapi md pthread z
+
+CFLAGS+= -I${BHYVE_SYSDIR}/sys/dev/usb/controller
 
 WARNS?=	2
 
diff --git a/atkbdc.c b/atkbdc.c
index 930b7af95f3d..8e71b0507cab 100644
--- a/atkbdc.c
+++ b/atkbdc.c
@@ -1,5 +1,6 @@
 /*-
  * Copyright (c) 2014 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * Copyright (c) 2015 Nahanni Systems Inc.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -35,56 +36,548 @@ __FBSDID("$FreeBSD$");
 
 #include <assert.h>
 #include <errno.h>
+#include <stdbool.h>
+#include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <pthread_np.h>
 
+#include "acpi.h"
 #include "inout.h"
+#include "pci_emul.h"
+#include "pci_irq.h"
 #include "pci_lpc.h"
+#include "ps2kbd.h"
+#include "ps2mouse.h"
 
 #define	KBD_DATA_PORT		0x60
 
 #define	KBD_STS_CTL_PORT	0x64
-#define	 KBD_SYS_FLAG		0x4
 
 #define	KBDC_RESET		0xfe
 
+#define	KBD_DEV_IRQ		1
+#define	AUX_DEV_IRQ		12
+
+/* controller commands */
+#define	KBDC_SET_COMMAND_BYTE	0x60
+#define	KBDC_GET_COMMAND_BYTE	0x20
+#define	KBDC_DISABLE_AUX_PORT	0xa7
+#define	KBDC_ENABLE_AUX_PORT	0xa8
+#define	KBDC_TEST_AUX_PORT	0xa9
+#define	KBDC_TEST_CTRL		0xaa
+#define	KBDC_TEST_KBD_PORT	0xab
+#define	KBDC_DISABLE_KBD_PORT	0xad
+#define	KBDC_ENABLE_KBD_PORT	0xae
+#define	KBDC_READ_INPORT	0xc0
+#define	KBDC_READ_OUTPORT	0xd0
+#define	KBDC_WRITE_OUTPORT	0xd1
+#define	KBDC_WRITE_KBD_OUTBUF	0xd2
+#define	KBDC_WRITE_AUX_OUTBUF	0xd3
+#define	KBDC_WRITE_TO_AUX	0xd4
+
+/* controller command byte (set by KBDC_SET_COMMAND_BYTE) */
+#define	KBD_TRANSLATION		0x40
+#define	KBD_SYS_FLAG_BIT	0x04
+#define	KBD_DISABLE_KBD_PORT	0x10
+#define	KBD_DISABLE_AUX_PORT	0x20
+#define	KBD_ENABLE_AUX_INT	0x02
+#define	KBD_ENABLE_KBD_INT	0x01
+#define	KBD_KBD_CONTROL_BITS	(KBD_DISABLE_KBD_PORT | KBD_ENABLE_KBD_INT)
+#define	KBD_AUX_CONTROL_BITS	(KBD_DISABLE_AUX_PORT | KBD_ENABLE_AUX_INT)
+
+/* controller status bits */
+#define	KBDS_KBD_BUFFER_FULL	0x01
+#define KBDS_SYS_FLAG		0x04
+#define KBDS_CTRL_FLAG		0x08
+#define	KBDS_AUX_BUFFER_FULL	0x20
+
+/* controller output port */
+#define	KBDO_KBD_OUTFULL	0x10
+#define	KBDO_AUX_OUTFULL	0x20
+
+#define	RAMSZ			32
+#define	FIFOSZ			15
+#define	CTRL_CMD_FLAG		0x8000
+
+struct kbd_dev {
+	bool	irq_active;
+	int	irq;
+
+	uint8_t	buffer[FIFOSZ];
+	int	brd, bwr;
+	int	bcnt;
+};
+
+struct aux_dev {
+	bool	irq_active;
+	int	irq;
+};
+
+struct atkbdc_softc {
+	struct vmctx *ctx;
+	pthread_mutex_t mtx;
+
+	struct ps2kbd_softc	*ps2kbd_sc;
+	struct ps2mouse_softc	*ps2mouse_sc;
+
+	uint8_t	status;		/* status register */
+	uint8_t	outport;	/* controller output port */
+	uint8_t	ram[RAMSZ];	/* byte0 = controller config */
+
+	uint32_t curcmd;	/* current command for next byte */
+	uint32_t  ctrlbyte;
+
+	struct kbd_dev kbd;
+	struct aux_dev aux;
+};
+
+static void
+atkbdc_assert_kbd_intr(struct atkbdc_softc *sc)
+{
+	if ((sc->ram[0] & KBD_ENABLE_KBD_INT) != 0) {
+		sc->kbd.irq_active = true;
+		vm_isa_pulse_irq(sc->ctx, sc->kbd.irq, sc->kbd.irq);
+	}
+}
+
+static void
+atkbdc_assert_aux_intr(struct atkbdc_softc *sc)
+{
+	if ((sc->ram[0] & KBD_ENABLE_AUX_INT) != 0) {
+		sc->aux.irq_active = true;
+		vm_isa_pulse_irq(sc->ctx, sc->aux.irq, sc->aux.irq);
+	}
+}
+
+static int
+atkbdc_kbd_queue_data(struct atkbdc_softc *sc, uint8_t val)
+{
+	assert(pthread_mutex_isowned_np(&sc->mtx));
+
+	if (sc->kbd.bcnt < FIFOSZ) {
+		sc->kbd.buffer[sc->kbd.bwr] = val;
+		sc->kbd.bwr = (sc->kbd.bwr + 1) % FIFOSZ;
+		sc->kbd.bcnt++;
+		sc->status |= KBDS_KBD_BUFFER_FULL;
+		sc->outport |= KBDO_KBD_OUTFULL;
+	} else {
+		printf("atkbd data buffer full\n");
+	}
+
+	return (sc->kbd.bcnt < FIFOSZ);
+}
+
+static void
+atkbdc_kbd_read(struct atkbdc_softc *sc)
+{
+	const uint8_t translation[256] = {
+		0xff, 0x43, 0x41, 0x3f, 0x3d, 0x3b, 0x3c, 0x58,
+		0x64, 0x44, 0x42, 0x40, 0x3e, 0x0f, 0x29, 0x59,
+		0x65, 0x38, 0x2a, 0x70, 0x1d, 0x10, 0x02, 0x5a,
+		0x66, 0x71, 0x2c, 0x1f, 0x1e, 0x11, 0x03, 0x5b,
+		0x67, 0x2e, 0x2d, 0x20, 0x12, 0x05, 0x04, 0x5c,
+		0x68, 0x39, 0x2f, 0x21, 0x14, 0x13, 0x06, 0x5d,
+		0x69, 0x31, 0x30, 0x23, 0x22, 0x15, 0x07, 0x5e,
+		0x6a, 0x72, 0x32, 0x24, 0x16, 0x08, 0x09, 0x5f,
+		0x6b, 0x33, 0x25, 0x17, 0x18, 0x0b, 0x0a, 0x60,
+		0x6c, 0x34, 0x35, 0x26, 0x27, 0x19, 0x0c, 0x61,
+		0x6d, 0x73, 0x28, 0x74, 0x1a, 0x0d, 0x62, 0x6e,
+		0x3a, 0x36, 0x1c, 0x1b, 0x75, 0x2b, 0x63, 0x76,
+		0x55, 0x56, 0x77, 0x78, 0x79, 0x7a, 0x0e, 0x7b,
+		0x7c, 0x4f, 0x7d, 0x4b, 0x47, 0x7e, 0x7f, 0x6f,
+		0x52, 0x53, 0x50, 0x4c, 0x4d, 0x48, 0x01, 0x45,
+		0x57, 0x4e, 0x51, 0x4a, 0x37, 0x49, 0x46, 0x54,
+		0x80, 0x81, 0x82, 0x41, 0x54, 0x85, 0x86, 0x87,
+		0x88, 0x89, 0x8a, 0x8b, 0x8c, 0x8d, 0x8e, 0x8f,
+		0x90, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97,
+		0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f,
+		0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
+		0xa8, 0xa9, 0xaa, 0xab, 0xac, 0xad, 0xae, 0xaf,
+		0xb0, 0xb1, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7,
+		0xb8, 0xb9, 0xba, 0xbb, 0xbc, 0xbd, 0xbe, 0xbf,
+		0xc0, 0xc1, 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7,
+		0xc8, 0xc9, 0xca, 0xcb, 0xcc, 0xcd, 0xce, 0xcf,
+		0xd0, 0xd1, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7,
+		0xd8, 0xd9, 0xda, 0xdb, 0xdc, 0xdd, 0xde, 0xdf,
+		0xe0, 0xe1, 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7,
+		0xe8, 0xe9, 0xea, 0xeb, 0xec, 0xed, 0xee, 0xef,
+		0xf0, 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7,
+		0xf8, 0xf9, 0xfa, 0xfb, 0xfc, 0xfd, 0xfe, 0xff
+	};
+	uint8_t val;
+	uint8_t release = 0;
+
+	assert(pthread_mutex_isowned_np(&sc->mtx));
+
+	if (sc->ram[0] & KBD_TRANSLATION) {
+		while (ps2kbd_read(sc->ps2kbd_sc, &val) != -1) {
+			if (val == 0xf0) {
+				release = 0x80;
+				continue;
+			} else {
+				val = translation[val] | release;
+			}
+			atkbdc_kbd_queue_data(sc, val);
+			break;
+		}
+	} else {
+		while (sc->kbd.bcnt < FIFOSZ) {
+			if (ps2kbd_read(sc->ps2kbd_sc, &val) != -1)
+				atkbdc_kbd_queue_data(sc, val);
+			else
+				break;
+		}
+	}
+
+	if (((sc->ram[0] & KBD_DISABLE_AUX_PORT) ||
+	    ps2mouse_fifocnt(sc->ps2mouse_sc) == 0) && sc->kbd.bcnt > 0)
+		atkbdc_assert_kbd_intr(sc);
+}
+
+static void
+atkbdc_aux_poll(struct atkbdc_softc *sc)
+{
+	if (ps2mouse_fifocnt(sc->ps2mouse_sc) > 0) {
+		sc->status |= KBDS_AUX_BUFFER_FULL | KBDS_KBD_BUFFER_FULL;
+		sc->outport |= KBDO_AUX_OUTFULL;
+		atkbdc_assert_aux_intr(sc);
+	}
+}
+
+static void
+atkbdc_kbd_poll(struct atkbdc_softc *sc)
+{
+	assert(pthread_mutex_isowned_np(&sc->mtx));
+
+	atkbdc_kbd_read(sc);
+}
+
+static void
+atkbdc_poll(struct atkbdc_softc *sc)
+{
+	atkbdc_aux_poll(sc);
+	atkbdc_kbd_poll(sc);
+}
+
+static void
+atkbdc_dequeue_data(struct atkbdc_softc *sc, uint8_t *buf)
+{
+	assert(pthread_mutex_isowned_np(&sc->mtx));
+
+	if (ps2mouse_read(sc->ps2mouse_sc, buf) == 0) {
+		if (ps2mouse_fifocnt(sc->ps2mouse_sc) == 0) {
+			if (sc->kbd.bcnt == 0)
+				sc->status &= ~(KBDS_AUX_BUFFER_FULL |
+				                KBDS_KBD_BUFFER_FULL);
+			else
+				sc->status &= ~(KBDS_AUX_BUFFER_FULL);
+			sc->outport &= ~KBDO_AUX_OUTFULL;
+		}
+
+		atkbdc_poll(sc);
+		return;
+	}
+
+	if (sc->kbd.bcnt > 0) {
+		*buf = sc->kbd.buffer[sc->kbd.brd];
+		sc->kbd.brd = (sc->kbd.brd + 1) % FIFOSZ;
+		sc->kbd.bcnt--;
+		if (sc->kbd.bcnt == 0) {
+			sc->status &= ~KBDS_KBD_BUFFER_FULL;
+			sc->outport &= ~KBDO_KBD_OUTFULL;
+		}
+
+		atkbdc_poll(sc);
+	}
+
+	if (ps2mouse_fifocnt(sc->ps2mouse_sc) == 0 && sc->kbd.bcnt == 0) {
+		sc->status &= ~(KBDS_AUX_BUFFER_FULL | KBDS_KBD_BUFFER_FULL);
+	}
+}
+
 static int
 atkbdc_data_handler(struct vmctx *ctx, int vcpu, int in, int port, int bytes,
     uint32_t *eax, void *arg)
 {
+	struct atkbdc_softc *sc;
+	uint8_t buf;
+	int retval;
+
 	if (bytes != 1)
 		return (-1);
+	sc = arg;
+	retval = 0;
+
+	pthread_mutex_lock(&sc->mtx);
+	if (in) {
+		sc->curcmd = 0;
+		if (sc->ctrlbyte != 0) {
+			*eax = sc->ctrlbyte & 0xff;
+			sc->ctrlbyte = 0;
+		} else {
+			/* read device buffer; includes kbd cmd responses */
+			atkbdc_dequeue_data(sc, &buf);
+			*eax = buf;
+		}
+
+		sc->status &= ~KBDS_CTRL_FLAG;
+		pthread_mutex_unlock(&sc->mtx);
+		return (retval);
+	}
+
+	if (sc->status & KBDS_CTRL_FLAG) {
+		/*
+		 * Command byte for the controller.
+		 */
+		switch (sc->curcmd) {
+		case KBDC_SET_COMMAND_BYTE:
+			sc->ram[0] = *eax;
+			if (sc->ram[0] & KBD_SYS_FLAG_BIT)
+				sc->status |= KBDS_SYS_FLAG;
+			else
+				sc->status &= ~KBDS_SYS_FLAG;
+			break;
+		case KBDC_WRITE_OUTPORT:
+			sc->outport = *eax;
+			break;
+		case KBDC_WRITE_TO_AUX:
+			ps2mouse_write(sc->ps2mouse_sc, *eax, 0);
+			atkbdc_poll(sc);
+			break;
+		case KBDC_WRITE_KBD_OUTBUF:
+			atkbdc_kbd_queue_data(sc, *eax);
+			break;
+		case KBDC_WRITE_AUX_OUTBUF:
+			ps2mouse_write(sc->ps2mouse_sc, *eax, 1);
+			sc->status |= (KBDS_AUX_BUFFER_FULL | KBDS_KBD_BUFFER_FULL);
+			atkbdc_aux_poll(sc);
+			break;
+		default:
+			/* write to particular RAM byte */
+			if (sc->curcmd >= 0x61 && sc->curcmd <= 0x7f) {
+				int byten;
+
+				byten = (sc->curcmd - 0x60) & 0x1f;
+				sc->ram[byten] = *eax & 0xff;
+			}
+			break;
+		}
 
-	*eax = 0;
+		sc->curcmd = 0;
+		sc->status &= ~KBDS_CTRL_FLAG;
+
+		pthread_mutex_unlock(&sc->mtx);
+		return (retval);
+	}
 
-	return (0);
+	/*
+	 * Data byte for the device.
+	 */
+	ps2kbd_write(sc->ps2kbd_sc, *eax);
+	atkbdc_poll(sc);
+
+	pthread_mutex_unlock(&sc->mtx);
+
+	return (retval);
 }
 
 static int
 atkbdc_sts_ctl_handler(struct vmctx *ctx, int vcpu, int in, int port,
     int bytes, uint32_t *eax, void *arg)
 {
-	int error, retval;
+	struct atkbdc_softc *sc;
+	int	error, retval;
 
 	if (bytes != 1)
 		return (-1);
 
+	sc = arg;
 	retval = 0;
+
+	pthread_mutex_lock(&sc->mtx);
+
 	if (in) {
-		*eax = KBD_SYS_FLAG;	/* system passed POST */
-	} else {
-		switch (*eax) {
-		case KBDC_RESET:	/* Pulse "reset" line. */
-			error = vm_suspend(ctx, VM_SUSPEND_RESET);
-			assert(error == 0 || errno == EALREADY);
-			break;
+		/* read status register */
+		*eax = sc->status;
+		pthread_mutex_unlock(&sc->mtx);
+		return (retval);
+	}
+
+
+	sc->curcmd = 0;
+	sc->status |= KBDS_CTRL_FLAG;
+	sc->ctrlbyte = 0;
+
+	switch (*eax) {
+	case KBDC_GET_COMMAND_BYTE:
+		sc->ctrlbyte = CTRL_CMD_FLAG | sc->ram[0];
+		break;
+	case KBDC_TEST_CTRL:
+		sc->ctrlbyte = CTRL_CMD_FLAG | 0x55;
+		break;
+	case KBDC_TEST_AUX_PORT:
+	case KBDC_TEST_KBD_PORT:
+		sc->ctrlbyte = CTRL_CMD_FLAG | 0;
+		break;
+	case KBDC_READ_INPORT:
+		sc->ctrlbyte = CTRL_CMD_FLAG | 0;
+		break;
+	case KBDC_READ_OUTPORT:
+		sc->ctrlbyte = CTRL_CMD_FLAG | sc->outport;
+		break;
+	case KBDC_SET_COMMAND_BYTE:
+	case KBDC_WRITE_OUTPORT:
+	case KBDC_WRITE_KBD_OUTBUF:
+	case KBDC_WRITE_AUX_OUTBUF:
+		sc->curcmd = *eax;
+		break;
+	case KBDC_DISABLE_KBD_PORT:
+		sc->ram[0] |= KBD_DISABLE_KBD_PORT;
+		break;
+	case KBDC_ENABLE_KBD_PORT:
+		sc->ram[0] &= ~KBD_DISABLE_KBD_PORT;
+		if (sc->kbd.bcnt > 0)
+			sc->status |= KBDS_KBD_BUFFER_FULL;
+		atkbdc_poll(sc);
+		break;
+	case KBDC_WRITE_TO_AUX:
+		sc->curcmd = *eax;
+		break;
+	case KBDC_DISABLE_AUX_PORT:
+		sc->ram[0] |= KBD_DISABLE_AUX_PORT;
+		ps2mouse_toggle(sc->ps2mouse_sc, 0);
+		sc->status &= ~(KBDS_AUX_BUFFER_FULL | KBDS_KBD_BUFFER_FULL);
+		sc->outport &= ~KBDS_AUX_BUFFER_FULL;
+		break;
+	case KBDC_ENABLE_AUX_PORT:
+		sc->ram[0] &= ~KBD_DISABLE_AUX_PORT;
+		ps2mouse_toggle(sc->ps2mouse_sc, 1);
+		if (ps2mouse_fifocnt(sc->ps2mouse_sc) > 0)
+			sc->status |= KBDS_AUX_BUFFER_FULL | KBDS_KBD_BUFFER_FULL;
+		break;
+	case KBDC_RESET:		/* Pulse "reset" line */
+		error = vm_suspend(ctx, VM_SUSPEND_RESET);
+		assert(error == 0 || errno == EALREADY);
+		break;
+	default:
+		if (*eax >= 0x21 && *eax <= 0x3f) {
+			/* read "byte N" from RAM */
+			int	byten;
+
+			byten = (*eax - 0x20) & 0x1f;
+			sc->ctrlbyte = CTRL_CMD_FLAG | sc->ram[byten];
 		}
+		break;
+	}
+
+	pthread_mutex_unlock(&sc->mtx);
+
+	if (sc->ctrlbyte != 0) {
+		sc->status |= KBDS_KBD_BUFFER_FULL;
+		sc->status &= ~KBDS_AUX_BUFFER_FULL;
+		atkbdc_assert_kbd_intr(sc);
+	} else if (ps2mouse_fifocnt(sc->ps2mouse_sc) > 0 &&
+	           (sc->ram[0] & KBD_DISABLE_AUX_PORT) == 0) {
+		sc->status |= KBDS_AUX_BUFFER_FULL | KBDS_KBD_BUFFER_FULL;
+		atkbdc_assert_aux_intr(sc);
+	} else if (sc->kbd.bcnt > 0 && (sc->ram[0] & KBD_DISABLE_KBD_PORT) == 0) {
+		sc->status |= KBDS_KBD_BUFFER_FULL;
+		atkbdc_assert_kbd_intr(sc);
 	}
 
 	return (retval);
 }
 
-INOUT_PORT(atkdbc, KBD_DATA_PORT, IOPORT_F_INOUT, atkbdc_data_handler);
-SYSRES_IO(KBD_DATA_PORT, 1);
-INOUT_PORT(atkbdc, KBD_STS_CTL_PORT,  IOPORT_F_INOUT,
-    atkbdc_sts_ctl_handler);
-SYSRES_IO(KBD_STS_CTL_PORT, 1);
+void
+atkbdc_event(struct atkbdc_softc *sc, int iskbd)
+{
+	pthread_mutex_lock(&sc->mtx);
+
+	if (iskbd)
+		atkbdc_kbd_poll(sc);
+	else
+		atkbdc_aux_poll(sc);
+	pthread_mutex_unlock(&sc->mtx);
+}
+
+void
+atkbdc_init(struct vmctx *ctx)
+{
+	struct inout_port iop;
+	struct atkbdc_softc *sc;
+	int error;
+
+	sc = calloc(1, sizeof(struct atkbdc_softc));
+	sc->ctx = ctx;
+
+	pthread_mutex_init(&sc->mtx, NULL);
+
+	bzero(&iop, sizeof(struct inout_port));
+	iop.name = "atkdbc";
+	iop.port = KBD_STS_CTL_PORT;
+	iop.size = 1;
+	iop.flags = IOPORT_F_INOUT;
+	iop.handler = atkbdc_sts_ctl_handler;
+	iop.arg = sc;
+
+	error = register_inout(&iop);
+	assert(error == 0);
+
+	bzero(&iop, sizeof(struct inout_port));
+	iop.name = "atkdbc";
+	iop.port = KBD_DATA_PORT;
+	iop.size = 1;
+	iop.flags = IOPORT_F_INOUT;
+	iop.handler = atkbdc_data_handler;
+	iop.arg = sc;
+
+	error = register_inout(&iop);
+	assert(error == 0);
+
+	pci_irq_reserve(KBD_DEV_IRQ);
+	sc->kbd.irq = KBD_DEV_IRQ;
+
+	pci_irq_reserve(AUX_DEV_IRQ);
+	sc->aux.irq = AUX_DEV_IRQ;
+
+	sc->ps2kbd_sc = ps2kbd_init(sc);
+	sc->ps2mouse_sc = ps2mouse_init(sc);
+}
+
+static void
+atkbdc_dsdt(void)
+{
+
+	dsdt_line("");
+	dsdt_line("Device (KBD)");
+	dsdt_line("{");
+	dsdt_line("  Name (_HID, EisaId (\"PNP0303\"))");
+	dsdt_line("  Name (_CRS, ResourceTemplate ()");
+	dsdt_line("  {");
+	dsdt_indent(2);
+	dsdt_fixed_ioport(KBD_DATA_PORT, 1);
+	dsdt_fixed_ioport(KBD_STS_CTL_PORT, 1);
+	dsdt_fixed_irq(1);
+	dsdt_unindent(2);
+	dsdt_line("  })");
+	dsdt_line("}");
+
+	dsdt_line("");
+	dsdt_line("Device (MOU)");
+	dsdt_line("{");
+	dsdt_line("  Name (_HID, EisaId (\"PNP0F13\"))");
+	dsdt_line("  Name (_CRS, ResourceTemplate ()");
+	dsdt_line("  {");
+	dsdt_indent(2);
+	dsdt_fixed_ioport(KBD_DATA_PORT, 1);
+	dsdt_fixed_ioport(KBD_STS_CTL_PORT, 1);
+	dsdt_fixed_irq(12);
+	dsdt_unindent(2);
+	dsdt_line("  })");
+	dsdt_line("}");
+}
+LPC_DSDT(atkbdc_dsdt);
+
diff --git a/atkbdc.h b/atkbdc.h
new file mode 100644
index 000000000000..85c8a7141eb2
--- /dev/null
+++ b/atkbdc.h
@@ -0,0 +1,38 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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$
+ */
+
+#ifndef _ATKBDC_H_
+#define _ATKBDC_H_
+
+struct atkbdc_softc;
+struct vmctx;
+
+void atkbdc_init(struct vmctx *ctx);
+void atkbdc_event(struct atkbdc_softc *sc, int iskbd);
+
+#endif /* _ATKBDC_H_ */
diff --git a/bhyvegc.c b/bhyvegc.c
new file mode 100644
index 000000000000..377b05b0878f
--- /dev/null
+++ b/bhyvegc.c
@@ -0,0 +1,72 @@
+#include <sys/cdefs.h>
+
+#include <sys/types.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "bhyvegc.h"
+
+struct bhyvegc {
+	struct bhyvegc_image	*gc_image;
+	int raw;
+};
+
+struct bhyvegc *
+bhyvegc_init(int width, int height, void *fbaddr)
+{
+	struct bhyvegc *gc;
+	struct bhyvegc_image *gc_image;
+
+	gc = calloc(1, sizeof (struct bhyvegc));
+
+	gc_image = calloc(1, sizeof(struct bhyvegc_image));
+	gc_image->width = width;
+	gc_image->height = height;
+	if (fbaddr) {
+		gc_image->data = fbaddr;
+		gc->raw = 1;
+	} else {
+		gc_image->data = calloc(width * height, sizeof (uint32_t));
+		gc->raw = 0;
+	}
+
+	gc->gc_image = gc_image;
+
+	return (gc);
+}
+
+void
+bhyvegc_set_fbaddr(struct bhyvegc *gc, void *fbaddr)
+{
+	gc->raw = 1;
+	if (gc->gc_image->data && gc->gc_image->data != fbaddr)
+		free(gc->gc_image->data);
+	gc->gc_image->data = fbaddr;
+}
+
+void
+bhyvegc_resize(struct bhyvegc *gc, int width, int height)
+{
+	struct bhyvegc_image *gc_image;
+
+	gc_image = gc->gc_image;
+
+	gc_image->width = width;
+	gc_image->height = height;
+	if (!gc->raw) {
+		gc_image->data = realloc(gc_image->data,
+		    sizeof (uint32_t) * width * height);
+		memset(gc_image->data, 0, width * height * sizeof (uint32_t));
+	}
+}
+
+struct bhyvegc_image *
+bhyvegc_get_image(struct bhyvegc *gc)
+{
+	if (gc == NULL)
+		return (NULL);
+
+	return (gc->gc_image);
+}
diff --git a/bhyvegc.h b/bhyvegc.h
new file mode 100644
index 000000000000..fa2ab68d9e2e
--- /dev/null
+++ b/bhyvegc.h
@@ -0,0 +1,46 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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$
+ */
+
+#ifndef _BHYVEGC_H_
+#define	_BHYVEGC_H_
+
+struct bhyvegc;
+
+struct bhyvegc_image {
+	int		vgamode;
+	int		width;
+	int		height;
+	uint32_t	*data;
+};
+
+struct bhyvegc *bhyvegc_init(int width, int height, void *fbaddr);
+void bhyvegc_set_fbaddr(struct bhyvegc *gc, void *fbaddr);
+void bhyvegc_resize(struct bhyvegc *gc, int width, int height);
+struct bhyvegc_image *bhyvegc_get_image(struct bhyvegc *gc);
+
+#endif /* _BHYVEGC_H_ */
diff --git a/bhyverun.c b/bhyverun.c
index bfa135b16cce..e62dc6887309 100644
--- a/bhyverun.c
+++ b/bhyverun.c
@@ -54,6 +54,7 @@ __FBSDID("$FreeBSD$");
 
 #include "bhyverun.h"
 #include "acpi.h"
+#include "atkbdc.h"
 #include "inout.h"
 #include "dbgport.h"
 #include "fwctl.h"
@@ -901,6 +902,7 @@ main(int argc, char *argv[])
 
 	init_mem();
 	init_inout();
+	atkbdc_init(ctx);
 	pci_irq_init(ctx);
 	ioapic_init(ctx);
 
diff --git a/console.c b/console.c
new file mode 100644
index 000000000000..ebb9c921bf02
--- /dev/null
+++ b/console.c
@@ -0,0 +1,118 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+
+#include "bhyvegc.h"
+#include "console.h"
+
+static struct {
+	struct bhyvegc		*gc;
+
+	fb_render_func_t	fb_render_cb;
+	void			*fb_arg;
+
+	kbd_event_func_t	kbd_event_cb;
+	void			*kbd_arg;
+	int			kbd_priority;
+
+	ptr_event_func_t	ptr_event_cb;
+	void			*ptr_arg;
+	int			ptr_priority;
+} console;
+
+void
+console_init(int w, int h, void *fbaddr)
+{
+	console.gc = bhyvegc_init(w, h, fbaddr);
+}
+
+void
+console_set_fbaddr(void *fbaddr)
+{
+	bhyvegc_set_fbaddr(console.gc, fbaddr);
+}
+
+struct bhyvegc_image *
+console_get_image(void)
+{
+	struct bhyvegc_image *bhyvegc_image;
+
+	bhyvegc_image = bhyvegc_get_image(console.gc);
+
+	return (bhyvegc_image);
+}
+
+void
+console_fb_register(fb_render_func_t render_cb, void *arg)
+{
+	console.fb_render_cb = render_cb;
+	console.fb_arg = arg;
+}
+
+void
+console_refresh(void)
+{
+	if (console.fb_render_cb)
+		(*console.fb_render_cb)(console.gc, console.fb_arg);
+}
+
+void
+console_kbd_register(kbd_event_func_t event_cb, void *arg, int pri)
+{
+	if (pri > console.kbd_priority) {
+		console.kbd_event_cb = event_cb;
+		console.kbd_arg = arg;
+		console.kbd_priority = pri;
+	}
+}
+
+void
+console_ptr_register(ptr_event_func_t event_cb, void *arg, int pri)
+{
+	if (pri > console.ptr_priority) {
+		console.ptr_event_cb = event_cb;
+		console.ptr_arg = arg;
+		console.ptr_priority = pri;
+	}
+}
+
+void
+console_key_event(int down, uint32_t keysym)
+{
+	if (console.kbd_event_cb)
+		(*console.kbd_event_cb)(down, keysym, console.kbd_arg);
+}
+
+void
+console_ptr_event(uint8_t button, int x, int y)
+{
+	if (console.ptr_event_cb)
+		(*console.ptr_event_cb)(button, x, y, console.ptr_arg);
+}
diff --git a/console.h b/console.h
new file mode 100644
index 000000000000..47193938a6f0
--- /dev/null
+++ b/console.h
@@ -0,0 +1,53 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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$
+ */
+
+#ifndef _CONSOLE_H_
+#define	_CONSOLE_H_
+
+struct bhyvegc;
+
+typedef void (*fb_render_func_t)(struct bhyvegc *gc, void *arg);
+typedef void (*kbd_event_func_t)(int down, uint32_t keysym, void *arg);
+typedef void (*ptr_event_func_t)(uint8_t mask, int x, int y, void *arg);
+
+void	console_init(int w, int h, void *fbaddr);
+
+void	console_set_fbaddr(void *fbaddr);
+
+struct bhyvegc_image *console_get_image(void);
+
+void	console_fb_register(fb_render_func_t render_cb, void *arg);
+void	console_refresh(void);
+
+void	console_kbd_register(kbd_event_func_t event_cb, void *arg, int pri);
+void	console_key_event(int down, uint32_t keysym);
+
+void	console_ptr_register(ptr_event_func_t event_cb, void *arg, int pri);
+void	console_ptr_event(uint8_t button, int x, int y);
+
+#endif /* _CONSOLE_H_ */
diff --git a/pci_fbuf.c b/pci_fbuf.c
new file mode 100644
index 000000000000..22ec86c5bd3b
--- /dev/null
+++ b/pci_fbuf.c
@@ -0,0 +1,406 @@
+/*-
+ * Copyright (c) 2015 Nahanni Systems, Inc.
+ * 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 ``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$
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+#include <sys/mman.h>
+
+#include <machine/vmm.h>
+#include <vmmapi.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <errno.h>
+#include <unistd.h>
+
+#include "bhyvegc.h"
+#include "bhyverun.h"
+#include "console.h"
+#include "inout.h"
+#include "pci_emul.h"
+#include "rfb.h"
+#include "vga.h"
+
+/*
+ * bhyve Framebuffer device emulation.
+ * BAR0 points to the current mode information.
+ * BAR1 is the 32-bit framebuffer address.
+ *
+ *  -s <b>,fbuf,wait,tcp=<ip>:port,w=width,h=height
+ */
+
+static int fbuf_debug = 1;
+#define	DEBUG_INFO	1
+#define	DEBUG_VERBOSE	4
+#define	DPRINTF(level, params)  if (level <= fbuf_debug) printf params
+
+
+#define	KB	(1024UL)
+#define	MB	(1024 * 1024UL)
+
+#define	DMEMSZ	128
+
+#define	FB_SIZE		(16*MB)
+
+#define COLS_MAX	1920
+#define	ROWS_MAX	1200
+
+#define COLS_DEFAULT	1024
+#define ROWS_DEFAULT	768
+
+#define COLS_MIN	640
+#define ROWS_MIN	480
+
+struct pci_fbuf_softc {
+	struct pci_devinst *fsc_pi;
+	struct {
+		uint32_t fbsize;
+		uint16_t width;
+		uint16_t height;
+		uint16_t depth;
+		uint16_t refreshrate;
+		uint8_t  reserved[116];
+	} __packed memregs;
+
+	/* rfb server */
+	char      *rfb_host;
+	int       rfb_port;
+	int       rfb_wait;
+	int       use_vga;
+
+	uint32_t  fbaddr;
+	char      *fb_base;
+	uint16_t  gc_width;
+	uint16_t  gc_height;
+	void      *vgasc;
+	struct bhyvegc_image *gc_image;
+};
+
+static struct pci_fbuf_softc *fbuf_sc;
+
+#define	PCI_FBUF_MSI_MSGS	 4
+
+static void
+pci_fbuf_usage(char *opt)
+{
+
+	fprintf(stderr, "Invalid fbuf emulation \"%s\"\r\n", opt);
+	fprintf(stderr, "fbuf: {wait,}tcp=<ip>:port\r\n");
+}
+
+static void
+pci_fbuf_write(struct vmctx *ctx, int vcpu, struct pci_devinst *pi,
+	       int baridx, uint64_t offset, int size, uint64_t value)
+{
+	struct pci_fbuf_softc *sc;
+	uint8_t *p;
+
+	assert(baridx == 0);
+
+	sc = pi->pi_arg;
+
+	DPRINTF(DEBUG_VERBOSE,
+	    ("fbuf wr: offset 0x%lx, size: %d, value: 0x%lx\n",
+	    offset, size, value));
+
+	if (offset + size > DMEMSZ) {
+		printf("fbuf: write too large, offset %ld size %d\n",
+		       offset, size);
+		return;
+	}
+
+	p = (uint8_t *)&sc->memregs + offset;
+
+	switch (size) {
+	case 1:
+		*p = value;
+		break;
+	case 2:
+		*(uint16_t *)p = value;
+		break;
+	case 4:
+		*(uint32_t *)p = value;
+		break;
+	case 8:
+		*(uint64_t *)p = value;
+		break;
+	default:
+		printf("fbuf: write unknown size %d\n", size);
+		break;
+	}
+
+	if (!sc->gc_image->vgamode && sc->memregs.width == 0 &&
+	    sc->memregs.height == 0) {
+		DPRINTF(DEBUG_INFO, ("switching to VGA mode\r\n"));
+		sc->gc_image->vgamode = 1;
+		sc->gc_width = 0;
+		sc->gc_height = 0;
+	} else if (sc->gc_image->vgamode && sc->memregs.width != 0 &&
+	    sc->memregs.height != 0) {
+		DPRINTF(DEBUG_INFO, ("switching to VESA mode\r\n"));
+		sc->gc_image->vgamode = 0;
+	}
+}
+
+uint64_t
+pci_fbuf_read(struct vmctx *ctx, int vcpu, struct pci_devinst *pi,
+	      int baridx, uint64_t offset, int size)
+{
+	struct pci_fbuf_softc *sc;
+	uint8_t *p;
+	uint64_t value;
+
+	assert(baridx == 0);
+
+	sc = pi->pi_arg;
+
+
+	if (offset + size > DMEMSZ) {
+		printf("fbuf: read too large, offset %ld size %d\n",
+		       offset, size);
+		return (0);
+	}
+
+	p = (uint8_t *)&sc->memregs + offset;
+	value = 0;
+	switch (size) {
+	case 1:
+		value = *p;
+		break;
+	case 2:
+		value = *(uint16_t *)p;
+		break;
+	case 4:
+		value = *(uint32_t *)p;
+		break;
+	case 8:
+		value = *(uint64_t *)p;
+		break;
+	default:
+		printf("fbuf: read unknown size %d\n", size);
+		break;
+	}
+
+	DPRINTF(DEBUG_VERBOSE,
+	    ("fbuf rd: offset 0x%lx, size: %d, value: 0x%lx\n",
+	     offset, size, value));
+
+	return (value);
+}
+
+static int
+pci_fbuf_parse_opts(struct pci_fbuf_softc *sc, char *opts)
+{
+	char	*uopts, *xopts, *config;
+	char	*tmpstr;
+	int	ret;
+
+	ret = 0;
+	uopts = strdup(opts);
+	for (xopts = strtok(uopts, ",");
+	     xopts != NULL;
+	     xopts = strtok(NULL, ",")) {
+		if (strcmp(xopts, "wait") == 0) {
+			sc->rfb_wait = 1;
+			continue;
+		}
+
+#if 0 /* notyet */		
+		if (strcmp(xopts, "vga") == 0) {
+			sc->use_vga = 1;
+			continue;
+		}
+#endif
+
+		if ((config = strchr(xopts, '=')) == NULL) {
+			pci_fbuf_usage(xopts);
+			ret = -1;
+			goto done;
+		}
+
+		*config++ = '\0';
+
+		DPRINTF(DEBUG_VERBOSE, ("pci_fbuf option %s = %s\r\n",
+		   xopts, config));
+
+		if (!strcmp(xopts, "tcp")) {
+			/* parse host-ip:port */
+			tmpstr = strsep(&config, ":");
+			if (!config)
+				sc->rfb_port = atoi(tmpstr);
+			else {
+				sc->rfb_port = atoi(config);
+				sc->rfb_host = tmpstr;
+			}
+		} else if (!strcmp(xopts, "w")) {
+			sc->memregs.width = atoi(config);
+			if (sc->memregs.width > COLS_MAX) {
+				pci_fbuf_usage(xopts);
+				ret = -1;
+				goto done;
+			} else if (sc->memregs.width == 0)
+				sc->memregs.width = 1920;
+		} else if (!strcmp(xopts, "h")) {
+			sc->memregs.height = atoi(config);
+			if (sc->memregs.height > ROWS_MAX) {
+				pci_fbuf_usage(xopts);
+				ret = -1;
+				goto done;
+			} else if (sc->memregs.height == 0)
+				sc->memregs.height = 1080;
+
+		} else {
+			pci_fbuf_usage(xopts);
+			ret = -1;
+			goto done;
+		}
+	}
+
+done:
+	return (ret);
+}
+
+
+extern void vga_render(struct bhyvegc *gc, void *arg);
+
+void
+pci_fbuf_render(struct bhyvegc *gc, void *arg)
+{
+	struct pci_fbuf_softc *sc;
+
+	sc = arg;
+
+	if (sc->use_vga && sc->gc_image->vgamode) {
+		/* TODO: mode switching to vga and vesa should use the special
+		 *      EFI-bhyve protocol port.
+		 */
+		vga_render(gc, sc->vgasc);
+		return;
+	}
+	if (sc->gc_width != sc->memregs.width ||
+	    sc->gc_height != sc->memregs.height) {
+		bhyvegc_resize(gc, sc->memregs.width, sc->memregs.height);
+		sc->gc_width = sc->memregs.width;
+		sc->gc_height = sc->memregs.height;
+	}
+
+	return;
+}
+
+static int
+pci_fbuf_init(struct vmctx *ctx, struct pci_devinst *pi, char *opts)
+{
+	int error, prot;
+	struct pci_fbuf_softc *sc;
+	
+	if (fbuf_sc != NULL) {
+		fprintf(stderr, "Only one frame buffer device is allowed.\n");
+		return (-1);
+	}
+
+	sc = calloc(1, sizeof(struct pci_fbuf_softc));
+
+	pi->pi_arg = sc;
+
+	/* initialize config space */
+	pci_set_cfgdata16(pi, PCIR_DEVICE, 0x40FB);
+	pci_set_cfgdata16(pi, PCIR_VENDOR, 0xFB5D);
+	pci_set_cfgdata8(pi, PCIR_CLASS, PCIC_DISPLAY);
+	pci_set_cfgdata8(pi, PCIR_SUBCLASS, PCIS_DISPLAY_VGA);
+
+	error = pci_emul_alloc_bar(pi, 0, PCIBAR_MEM32, DMEMSZ);
+	assert(error == 0);
+
+	error = pci_emul_alloc_bar(pi, 1, PCIBAR_MEM32, FB_SIZE);
+	assert(error == 0);
+
+	error = pci_emul_add_msicap(pi, PCI_FBUF_MSI_MSGS);
+	assert(error == 0);
+
+	sc->fbaddr = pi->pi_bar[1].addr;
+	sc->memregs.fbsize = FB_SIZE;
+	sc->memregs.width  = COLS_DEFAULT;
+	sc->memregs.height = ROWS_DEFAULT;
+	sc->memregs.depth  = 32;
+
+	sc->fsc_pi = pi;
+
+	error = pci_fbuf_parse_opts(sc, opts);
+	if (error != 0)
+		goto done;
+
+	sc->fb_base = vm_create_devmem(ctx, VM_FRAMEBUFFER, "framebuffer", FB_SIZE);
+	if (sc->fb_base == MAP_FAILED) {
+		error = -1;
+		goto done;
+	}
+	DPRINTF(DEBUG_INFO, ("fbuf frame buffer base: %p [sz %lu]\r\n",
+	        sc->fb_base, FB_SIZE));
+
+	/*
+	 * Map the framebuffer into the guest address space.
+	 * XXX This may fail if the BAR is different than a prior
+	 * run. In this case flag the error. This will be fixed
+	 * when a change_memseg api is available.
+	 */
+	prot = PROT_READ | PROT_WRITE;
+	if (vm_mmap_memseg(ctx, sc->fbaddr, VM_FRAMEBUFFER, 0, FB_SIZE, prot) != 0) {
+		fprintf(stderr, "pci_fbuf: mapseg failed - try deleting VM and restarting\n");
+		error = -1;
+		goto done;
+	}
+
+	console_init(sc->memregs.width, sc->memregs.height, sc->fb_base);
+	console_fb_register(pci_fbuf_render, sc);
+
+	sc->vgasc = vga_init(!sc->use_vga);
+	sc->gc_image = console_get_image();
+
+	fbuf_sc = sc;
+
+	memset((void *)sc->fb_base, 0, FB_SIZE);
+
+	error = rfb_init(sc->rfb_host, sc->rfb_port, sc->rfb_wait);
+done:
+	if (error)
+		free(sc);
+
+	return (error);
+}
+
+struct pci_devemu pci_fbuf = {
+	.pe_emu =	"fbuf",
+	.pe_init =	pci_fbuf_init,
+	.pe_barwrite =	pci_fbuf_write,
+	.pe_barread =	pci_fbuf_read
+};
+PCI_EMUL_SET(pci_fbuf);
diff --git a/pci_xhci.c b/pci_xhci.c
new file mode 100644
index 000000000000..416226821f6a
--- /dev/null
+++ b/pci_xhci.c
@@ -0,0 +1,2828 @@
+/*-
+ * Copyright (c) 2014 Leon Dang <ldang@nahannisys.com>
+ * 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.
+ */
+/*
+   XHCI options:
+    -s <n>,xhci,{devices}
+
+   devices:
+     ums             USB tablet mouse
+ */
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/uio.h>
+#include <sys/types.h>
+#include <sys/queue.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+#include <pthread.h>
+#include <unistd.h>
+
+#include <dev/usb/usbdi.h>
+#include <dev/usb/usb.h>
+#include <dev/usb/usb_freebsd.h>
+#include <xhcireg.h>
+
+#include "bhyverun.h"
+#include "pci_emul.h"
+#include "pci_xhci.h"
+#include "usb_emul.h"
+
+
+static int xhci_debug = 0;
+#define	DPRINTF(params) if (xhci_debug) printf params
+#define	WPRINTF(params) printf params
+
+
+#define	XHCI_NAME		"xhci"
+#define	XHCI_MAX_DEVS		8	/* 4 USB3 + 4 USB2 devs */
+
+#define	XHCI_MAX_SLOTS		64	/* min allowed by Windows drivers */
+
+/*
+ * XHCI data structures can be up to 64k, but limit paddr_guest2host mapping
+ * to 4k to avoid going over the guest physical memory barrier.
+ */
+#define	XHCI_PADDR_SZ		4096	/* paddr_guest2host max size */
+
+#define	XHCI_ERST_MAX		0	/* max 2^entries event ring seg tbl */
+
+#define	XHCI_CAPLEN		(4*8)	/* offset of op register space */
+#define	XHCI_HCCPRAMS2		0x1C	/* offset of HCCPARAMS2 register */
+#define	XHCI_PORTREGS_START	0x400
+#define	XHCI_DOORBELL_MAX	256
+
+#define	XHCI_STREAMS_MAX	1	/* 4-15 in XHCI spec */
+
+/* caplength and hci-version registers */
+#define	XHCI_SET_CAPLEN(x)		((x) & 0xFF)
+#define	XHCI_SET_HCIVERSION(x)		(((x) & 0xFFFF) << 16)
+#define	XHCI_GET_HCIVERSION(x)		(((x) >> 16) & 0xFFFF)
+
+/* hcsparams1 register */
+#define	XHCI_SET_HCSP1_MAXSLOTS(x)	((x) & 0xFF)
+#define	XHCI_SET_HCSP1_MAXINTR(x)	(((x) & 0x7FF) << 8)
+#define	XHCI_SET_HCSP1_MAXPORTS(x)	(((x) & 0xFF) << 24)
+
+/* hcsparams2 register */
+#define	XHCI_SET_HCSP2_IST(x)		((x) & 0x0F)
+#define	XHCI_SET_HCSP2_ERSTMAX(x)	(((x) & 0x0F) << 4)
+#define	XHCI_SET_HCSP2_MAXSCRATCH_HI(x)	(((x) & 0x1F) << 21)
+#define	XHCI_SET_HCSP2_MAXSCRATCH_LO(x)	(((x) & 0x1F) << 27)
+
+/* hcsparams3 register */
+#define	XHCI_SET_HCSP3_U1EXITLATENCY(x)	((x) & 0xFF)
+#define	XHCI_SET_HCSP3_U2EXITLATENCY(x)	(((x) & 0xFFFF) << 16)
+
+/* hccparams1 register */
+#define	XHCI_SET_HCCP1_AC64(x)		((x) & 0x01)
+#define	XHCI_SET_HCCP1_BNC(x)		(((x) & 0x01) << 1)
+#define	XHCI_SET_HCCP1_CSZ(x)		(((x) & 0x01) << 2)
+#define	XHCI_SET_HCCP1_PPC(x)		(((x) & 0x01) << 3)
+#define	XHCI_SET_HCCP1_PIND(x)		(((x) & 0x01) << 4)
+#define	XHCI_SET_HCCP1_LHRC(x)		(((x) & 0x01) << 5)
+#define	XHCI_SET_HCCP1_LTC(x)		(((x) & 0x01) << 6)
+#define	XHCI_SET_HCCP1_NSS(x)		(((x) & 0x01) << 7)
+#define	XHCI_SET_HCCP1_PAE(x)		(((x) & 0x01) << 8)
+#define	XHCI_SET_HCCP1_SPC(x)		(((x) & 0x01) << 9)
+#define	XHCI_SET_HCCP1_SEC(x)		(((x) & 0x01) << 10)
+#define	XHCI_SET_HCCP1_CFC(x)		(((x) & 0x01) << 11)
+#define	XHCI_SET_HCCP1_MAXPSA(x)	(((x) & 0x0F) << 12)
+#define	XHCI_SET_HCCP1_XECP(x)		(((x) & 0xFFFF) << 16)
+
+/* hccparams2 register */
+#define	XHCI_SET_HCCP2_U3C(x)		((x) & 0x01)
+#define	XHCI_SET_HCCP2_CMC(x)		(((x) & 0x01) << 1)
+#define	XHCI_SET_HCCP2_FSC(x)		(((x) & 0x01) << 2)
+#define	XHCI_SET_HCCP2_CTC(x)		(((x) & 0x01) << 3)
+#define	XHCI_SET_HCCP2_LEC(x)		(((x) & 0x01) << 4)
+#define	XHCI_SET_HCCP2_CIC(x)		(((x) & 0x01) << 5)
+
+/* other registers */
+#define	XHCI_SET_DOORBELL(x)		((x) & ~0x03)
+#define	XHCI_SET_RTSOFFSET(x)		((x) & ~0x0F)
+
+/* register masks */
+#define	XHCI_PS_PLS_MASK		(0xF << 5)	/* port link state */
+#define	XHCI_PS_SPEED_MASK		(0xF << 10)	/* port speed */
+#define	XHCI_PS_PIC_MASK		(0x3 << 14)	/* port indicator */
+
+/* port register set */
+#define	XHCI_PORTREGS_BASE		0x400		/* base offset */
+#define	XHCI_PORTREGS_PORT0		0x3F0
+#define	XHCI_PORTREGS_SETSZ		0x10		/* size of a set */
+
+#define	MASK_64_HI(x)			((x) & ~0xFFFFFFFFULL)
+#define	MASK_64_LO(x)			((x) & 0xFFFFFFFFULL)
+
+#define	FIELD_REPLACE(a,b,m,s)		((a) & ~((m) << (s)) | \
+					((b) & (m)) << (s))
+#define	FIELD_COPY(a,b,m,s)		((a) & ~((m) << (s)) | \
+					((b) & ((m) << (s))))
+
+struct pci_xhci_trb_ring {
+	uint64_t ringaddr;		/* current dequeue guest address */
+	uint32_t ccs;			/* consumer cycle state */
+};
+
+/* device endpoint transfer/stream rings */
+struct pci_xhci_dev_ep {
+	union {
+		struct xhci_trb		*_epu_tr;
+		struct xhci_stream_ctx	*_epu_sctx;
+	} _ep_trbsctx;
+#define	ep_tr		_ep_trbsctx._epu_tr
+#define	ep_sctx		_ep_trbsctx._epu_sctx
+
+	union {
+		struct pci_xhci_trb_ring _epu_trb;
+		struct pci_xhci_trb_ring *_epu_sctx_trbs;
+	} _ep_trb_rings;
+#define	ep_ringaddr	_ep_trb_rings._epu_trb.ringaddr
+#define	ep_ccs		_ep_trb_rings._epu_trb.ccs
+#define	ep_sctx_trbs	_ep_trb_rings._epu_sctx_trbs
+
+	struct usb_data_xfer *ep_xfer;	/* transfer chain */
+};
+
+/* device context base address array: maps slot->device context */
+struct xhci_dcbaa {
+	uint64_t dcba[USB_MAX_DEVICES+1]; /* xhci_dev_ctx ptrs */
+};
+
+/* port status registers */
+struct pci_xhci_portregs {
+	uint32_t	portsc;		/* port status and control */
+	uint32_t	portpmsc;	/* port pwr mgmt status & control */
+	uint32_t	portli;		/* port link info */
+	uint32_t	porthlpmc;	/* port hardware LPM control */
+} __packed;
+#define	XHCI_PS_SPEED_SET(x)	(((x) & 0xF) << 10)
+
+/* xHC operational registers */
+struct pci_xhci_opregs {
+	uint32_t	usbcmd;		/* usb command */
+	uint32_t	usbsts;		/* usb status */
+	uint32_t	pgsz;		/* page size */
+	uint32_t	dnctrl;		/* device notification control */
+	uint64_t	crcr;		/* command ring control */
+	uint64_t	dcbaap;		/* device ctx base addr array ptr */
+	uint32_t	config;		/* configure */
+
+	/* guest mapped addresses: */
+	struct xhci_trb	*cr_p;		/* crcr dequeue */
+	struct xhci_dcbaa *dcbaa_p;	/* dev ctx array ptr */
+};
+
+/* xHC runtime registers */
+struct pci_xhci_rtsregs {
+	uint32_t	mfindex;	/* microframe index */
+	struct {			/* interrupter register set */
+		uint32_t	iman;	/* interrupter management */
+		uint32_t	imod;	/* interrupter moderation */
+		uint32_t	erstsz;	/* event ring segment table size */
+		uint32_t	rsvd;
+		uint64_t	erstba;	/* event ring seg-tbl base addr */
+		uint64_t	erdp;	/* event ring dequeue ptr */
+	} intrreg __packed;
+
+	/* guest mapped addresses */
+	struct xhci_event_ring_seg *erstba_p;
+	struct xhci_trb *erst_p;	/* event ring segment tbl */
+	int		er_deq_seg;	/* event ring dequeue segment */
+	int		er_enq_idx;	/* event ring enqueue index - xHCI */
+	int		er_enq_seg;	/* event ring enqueue segment */
+	uint32_t	er_events_cnt;	/* number of events in ER */
+	uint32_t	event_pcs;	/* producer cycle state flag */
+};
+
+
+struct pci_xhci_softc;
+
+
+/*
+ * USB device emulation container.
+ * This is referenced from usb_hci->hci_sc; 1 pci_xhci_dev_emu for each
+ * emulated device instance.
+ */
+struct pci_xhci_dev_emu {
+	struct pci_xhci_softc	*xsc;
+
+	/* XHCI contexts */
+	struct xhci_dev_ctx	*dev_ctx;
+	struct pci_xhci_dev_ep	eps[XHCI_MAX_ENDPOINTS];
+	int			dev_slotstate;
+
+	struct usb_devemu	*dev_ue;	/* USB emulated dev */
+	void			*dev_sc;	/* device's softc */
+
+	struct usb_hci		hci;
+};
+
+struct pci_xhci_softc {
+	struct pci_devinst *xsc_pi;
+
+	pthread_mutex_t	mtx;
+
+	uint32_t	caplength;	/* caplen & hciversion */
+	uint32_t	hcsparams1;	/* structural parameters 1 */
+	uint32_t	hcsparams2;	/* structural parameters 2 */
+	uint32_t	hcsparams3;	/* structural parameters 3 */
+	uint32_t	hccparams1;	/* capability parameters 1 */
+	uint32_t	dboff;		/* doorbell offset */
+	uint32_t	rtsoff;		/* runtime register space offset */
+	uint32_t	hccparams2;	/* capability parameters 2 */
+
+	uint32_t	regsend;	/* end of configuration registers */
+
+	struct pci_xhci_opregs  opregs;
+	struct pci_xhci_rtsregs rtsregs;
+
+	struct pci_xhci_portregs *portregs;
+	struct pci_xhci_dev_emu  **devices; /* XHCI[port] = device */
+	struct pci_xhci_dev_emu  **slots;   /* slots assigned from 1 */
+	int		ndevices;
+
+	int		usb2_port_start;
+	int		usb3_port_start;
+};
+
+
+/* portregs and devices arrays are set up to start from idx=1 */
+#define	XHCI_PORTREG_PTR(x,n)	&(x)->portregs[(n)]
+#define	XHCI_DEVINST_PTR(x,n)	(x)->devices[(n)]
+#define	XHCI_SLOTDEV_PTR(x,n)	(x)->slots[(n)]
+
+#define	XHCI_HALTED(sc)		((sc)->opregs.usbsts & XHCI_STS_HCH)
+
+#define	XHCI_GADDR(sc,a)	paddr_guest2host((sc)->xsc_pi->pi_vmctx, \
+				    (a),                                 \
+				    XHCI_PADDR_SZ - ((a) & (XHCI_PADDR_SZ-1)))
+
+static int xhci_in_use;
+
+/* map USB errors to XHCI */
+static const int xhci_usb_errors[USB_ERR_MAX] = {
+	[USB_ERR_NORMAL_COMPLETION]	= XHCI_TRB_ERROR_SUCCESS,
+	[USB_ERR_PENDING_REQUESTS]	= XHCI_TRB_ERROR_RESOURCE,
+	[USB_ERR_NOT_STARTED]		= XHCI_TRB_ERROR_ENDP_NOT_ON,
+	[USB_ERR_INVAL]			= XHCI_TRB_ERROR_INVALID,
+	[USB_ERR_NOMEM]			= XHCI_TRB_ERROR_RESOURCE,
+	[USB_ERR_CANCELLED]		= XHCI_TRB_ERROR_STOPPED,
+	[USB_ERR_BAD_ADDRESS]		= XHCI_TRB_ERROR_PARAMETER,
+	[USB_ERR_BAD_BUFSIZE]		= XHCI_TRB_ERROR_PARAMETER,
+	[USB_ERR_BAD_FLAG]		= XHCI_TRB_ERROR_PARAMETER,
+	[USB_ERR_NO_CALLBACK]		= XHCI_TRB_ERROR_STALL,
+	[USB_ERR_IN_USE]		= XHCI_TRB_ERROR_RESOURCE,
+	[USB_ERR_NO_ADDR]		= XHCI_TRB_ERROR_RESOURCE,
+	[USB_ERR_NO_PIPE]               = XHCI_TRB_ERROR_RESOURCE,
+	[USB_ERR_ZERO_NFRAMES]          = XHCI_TRB_ERROR_UNDEFINED,
+	[USB_ERR_ZERO_MAXP]             = XHCI_TRB_ERROR_UNDEFINED,
+	[USB_ERR_SET_ADDR_FAILED]       = XHCI_TRB_ERROR_RESOURCE,
+	[USB_ERR_NO_POWER]              = XHCI_TRB_ERROR_ENDP_NOT_ON,
+	[USB_ERR_TOO_DEEP]              = XHCI_TRB_ERROR_RESOURCE,
+	[USB_ERR_IOERROR]               = XHCI_TRB_ERROR_TRB,
+	[USB_ERR_NOT_CONFIGURED]        = XHCI_TRB_ERROR_ENDP_NOT_ON,
+	[USB_ERR_TIMEOUT]               = XHCI_TRB_ERROR_CMD_ABORTED,
+	[USB_ERR_SHORT_XFER]            = XHCI_TRB_ERROR_SHORT_PKT,
+	[USB_ERR_STALLED]               = XHCI_TRB_ERROR_STALL,
+	[USB_ERR_INTERRUPTED]           = XHCI_TRB_ERROR_CMD_ABORTED,
+	[USB_ERR_DMA_LOAD_FAILED]       = XHCI_TRB_ERROR_DATA_BUF,
+	[USB_ERR_BAD_CONTEXT]           = XHCI_TRB_ERROR_TRB,
+	[USB_ERR_NO_ROOT_HUB]           = XHCI_TRB_ERROR_UNDEFINED,
+	[USB_ERR_NO_INTR_THREAD]        = XHCI_TRB_ERROR_UNDEFINED,
+	[USB_ERR_NOT_LOCKED]            = XHCI_TRB_ERROR_UNDEFINED,
+};
+#define	USB_TO_XHCI_ERR(e)	((e) < USB_ERR_MAX ? xhci_usb_errors[(e)] : \
+				XHCI_TRB_ERROR_INVALID)
+
+static int pci_xhci_insert_event(struct pci_xhci_softc *sc,
+    struct xhci_trb *evtrb, int do_intr);
+static void pci_xhci_dump_trb(struct xhci_trb *trb);
+static void pci_xhci_assert_interrupt(struct pci_xhci_softc *sc);
+static void pci_xhci_reset_slot(struct pci_xhci_softc *sc, int slot);
+static void pci_xhci_reset_port(struct pci_xhci_softc *sc, int portn, int warm);
+static void pci_xhci_update_ep_ring(struct pci_xhci_softc *sc,
+    struct pci_xhci_dev_emu *dev, struct pci_xhci_dev_ep *devep,
+    struct xhci_endp_ctx *ep_ctx, uint32_t streamid,
+    uint64_t ringaddr, int ccs);
+
+static void
+pci_xhci_set_evtrb(struct xhci_trb *evtrb, uint64_t port, uint32_t errcode,
+    uint32_t evtype)
+{
+	evtrb->qwTrb0 = port << 24;
+	evtrb->dwTrb2 = XHCI_TRB_2_ERROR_SET(errcode);
+	evtrb->dwTrb3 = XHCI_TRB_3_TYPE_SET(evtype);
+}
+
+
+/* controller reset */
+static void
+pci_xhci_reset(struct pci_xhci_softc *sc)
+{
+	int i;
+
+	sc->rtsregs.er_enq_idx = 0;
+	sc->rtsregs.er_events_cnt = 0;
+	sc->rtsregs.event_pcs = 1;
+
+	for (i = 1; i <= XHCI_MAX_SLOTS; i++) {
+		pci_xhci_reset_slot(sc, i);
+	}
+}
+
+static uint32_t
+pci_xhci_usbcmd_write(struct pci_xhci_softc *sc, uint32_t cmd)
+{
+	int do_intr = 0;
+	int i;
+
+	if (cmd & XHCI_CMD_RS) {
+		do_intr = (sc->opregs.usbcmd & XHCI_CMD_RS) == 0;
+
+		sc->opregs.usbcmd |= XHCI_CMD_RS;
+		sc->opregs.usbsts &= ~XHCI_STS_HCH;
+		sc->opregs.usbsts |= XHCI_STS_PCD;
+
+		/* Queue port change event on controller run from stop */
+		if (do_intr)
+			for (i = 1; i <= XHCI_MAX_DEVS; i++) {
+				struct pci_xhci_dev_emu *dev;
+				struct pci_xhci_portregs *port;
+				struct xhci_trb		evtrb;
+
+				if ((dev = XHCI_DEVINST_PTR(sc, i)) == NULL)
+					continue;
+
+				port = XHCI_PORTREG_PTR(sc, i);
+				port->portsc |= XHCI_PS_CSC | XHCI_PS_CCS;
+				port->portsc &= ~XHCI_PS_PLS_MASK;
+
+				/*
+				 * XHCI 4.19.3 USB2 RxDetect->Polling,
+				 *             USB3 Polling->U0
+				 */
+				if (dev->dev_ue->ue_usbver == 2)
+					port->portsc |=
+					    XHCI_PS_PLS_SET(UPS_PORT_LS_POLL);
+				else
+					port->portsc |=
+					    XHCI_PS_PLS_SET(UPS_PORT_LS_U0);
+
+				pci_xhci_set_evtrb(&evtrb, i,
+				    XHCI_TRB_ERROR_SUCCESS,
+				    XHCI_TRB_EVENT_PORT_STS_CHANGE);
+
+				if (pci_xhci_insert_event(sc, &evtrb, 0) !=
+				    XHCI_TRB_ERROR_SUCCESS)
+					break;
+			}
+	} else {
+		sc->opregs.usbcmd &= ~XHCI_CMD_RS;
+		sc->opregs.usbsts |= XHCI_STS_HCH;
+		sc->opregs.usbsts &= ~XHCI_STS_PCD;
+	}
+
+	/* start execution of schedule; stop when set to 0 */
+	cmd |= sc->opregs.usbcmd & XHCI_CMD_RS;
+
+	if (cmd & XHCI_CMD_HCRST) {
+		/* reset controller */
+		pci_xhci_reset(sc);
+		cmd &= ~XHCI_CMD_HCRST;
+	}
+
+	cmd &= ~(XHCI_CMD_CSS | XHCI_CMD_CRS);
+
+	if (do_intr)
+		pci_xhci_assert_interrupt(sc);
+
+	return (cmd);
+}
+
+static void
+pci_xhci_portregs_write(struct pci_xhci_softc *sc, uint64_t offset,
+    uint64_t value)
+{
+	struct xhci_trb		evtrb;
+	struct pci_xhci_portregs *p;
+	int port;
+	uint32_t oldpls, newpls;
+
+	if (sc->portregs == NULL)
+		return;
+
+	port = (offset - XHCI_PORTREGS_PORT0) / XHCI_PORTREGS_SETSZ;
+	offset = (offset - XHCI_PORTREGS_PORT0) % XHCI_PORTREGS_SETSZ;
+
+	DPRINTF(("pci_xhci: portregs wr offset 0x%lx, port %u: 0x%lx\r\n",
+	        offset, port, value));
+
+	assert(port >= 0);
+
+	if (port > XHCI_MAX_DEVS) {
+		DPRINTF(("pci_xhci: portregs_write port %d > ndevices\r\n",
+		    port));
+		return;
+	}
+
+	if (XHCI_DEVINST_PTR(sc, port) == NULL) {
+		DPRINTF(("pci_xhci: portregs_write to unattached port %d\r\n",
+		     port));
+	}
+
+	p = XHCI_PORTREG_PTR(sc, port);
+	switch (offset) {
+	case 0:
+		/* port reset or warm reset */
+		if (value & (XHCI_PS_PR | XHCI_PS_WPR)) {
+			pci_xhci_reset_port(sc, port, value & XHCI_PS_WPR);
+			break;
+		}
+
+		if ((p->portsc & XHCI_PS_PP) == 0) {
+			WPRINTF(("pci_xhci: portregs_write to unpowered "
+			         "port %d\r\n", port));
+			break;
+		}
+
+		/* Port status and control register  */
+		oldpls = XHCI_PS_PLS_GET(p->portsc);
+		newpls = XHCI_PS_PLS_GET(value);
+
+		p->portsc &= XHCI_PS_PED | XHCI_PS_PLS_MASK |
+		             XHCI_PS_SPEED_MASK | XHCI_PS_PIC_MASK;
+  
+		if (XHCI_DEVINST_PTR(sc, port))
+			p->portsc |= XHCI_PS_CCS;
+
+		p->portsc |= (value &
+		              ~(XHCI_PS_OCA |
+		                XHCI_PS_PR  |
+			        XHCI_PS_PED |
+			        XHCI_PS_PLS_MASK   |	/* link state */
+			        XHCI_PS_SPEED_MASK |
+			        XHCI_PS_PIC_MASK   |	/* port indicator */
+			        XHCI_PS_LWS | XHCI_PS_DR | XHCI_PS_WPR));
+
+		/* clear control bits */
+		p->portsc &= ~(value &
+		               (XHCI_PS_CSC |
+		                XHCI_PS_PEC |
+		                XHCI_PS_WRC |
+		                XHCI_PS_OCC |
+		                XHCI_PS_PRC |
+		                XHCI_PS_PLC |
+		                XHCI_PS_CEC |
+		                XHCI_PS_CAS));
+
+		/* port disable request; for USB3, don't care */
+		if (value & XHCI_PS_PED)
+			DPRINTF(("Disable port %d request\r\n", port));
+
+		if (!(value & XHCI_PS_LWS))
+			break;
+
+		DPRINTF(("Port new PLS: %d\r\n", newpls));
+		switch (newpls) {
+		case 0: /* U0 */
+		case 3: /* U3 */
+			if (oldpls != newpls) {
+				p->portsc &= ~XHCI_PS_PLS_MASK;
+				p->portsc |= XHCI_PS_PLS_SET(newpls) |
+				             XHCI_PS_PLC;
+
+				if (oldpls != 0 && newpls == 0) {
+					pci_xhci_set_evtrb(&evtrb, port,
+					    XHCI_TRB_ERROR_SUCCESS,
+					    XHCI_TRB_EVENT_PORT_STS_CHANGE);
+
+					pci_xhci_insert_event(sc, &evtrb, 1);
+				}
+			}
+			break;
+
+		default:
+			DPRINTF(("Unhandled change port %d PLS %u\r\n",
+			         port, newpls));
+			break;
+		}
+		break;
+	case 4: 
+		/* Port power management status and control register  */
+		p->portpmsc = value;
+		break;
+	case 8:
+		/* Port link information register */
+		DPRINTF(("pci_xhci attempted write to PORTLI, port %d\r\n",
+		        port));
+		break;
+	case 12:
+		/*
+		 * Port hardware LPM control register.
+		 * For USB3, this register is reserved.
+		 */
+		p->porthlpmc = value;
+		break;
+	}
+}
+
+struct xhci_dev_ctx *
+pci_xhci_get_dev_ctx(struct pci_xhci_softc *sc, uint32_t slot)
+{
+	uint64_t devctx_addr;
+	struct xhci_dev_ctx *devctx;
+
+	assert(slot > 0 && slot <= sc->ndevices);
+	assert(sc->opregs.dcbaa_p != NULL);
+
+	devctx_addr = sc->opregs.dcbaa_p->dcba[slot];
+
+	if (devctx_addr == 0) {
+		DPRINTF(("get_dev_ctx devctx_addr == 0\r\n"));
+		return (NULL);
+	}
+
+	DPRINTF(("pci_xhci: get dev ctx, slot %u devctx addr %016lx\r\n",
+	        slot, devctx_addr));
+	devctx = XHCI_GADDR(sc, devctx_addr & ~0x3FUL);
+
+	return (devctx);
+}
+
+struct xhci_trb *
+pci_xhci_trb_next(struct pci_xhci_softc *sc, struct xhci_trb *curtrb,
+    uint64_t *guestaddr)
+{
+	struct xhci_trb *next;
+
+	assert(curtrb != NULL);
+
+	if (XHCI_TRB_3_TYPE_GET(curtrb->dwTrb3) == XHCI_TRB_TYPE_LINK) {
+		if (guestaddr)
+			*guestaddr = curtrb->qwTrb0 & ~0xFUL;
+		
+		next = XHCI_GADDR(sc, curtrb->qwTrb0 & ~0xFUL);
+	} else {
+		if (guestaddr)
+			*guestaddr += sizeof(struct xhci_trb) & ~0xFUL;
+
+		next = curtrb + 1;
+	}
+
+	return (next);
+}
+
+static void
+pci_xhci_assert_interrupt(struct pci_xhci_softc *sc)
+{
+
+	sc->rtsregs.intrreg.erdp |= XHCI_ERDP_LO_BUSY;
+	sc->rtsregs.intrreg.iman |= XHCI_IMAN_INTR_PEND;
+	sc->opregs.usbsts |= XHCI_STS_EINT;
+
+	/* only trigger interrupt if permitted */
+	if ((sc->opregs.usbcmd & XHCI_CMD_INTE) &&
+	    (sc->rtsregs.intrreg.iman & XHCI_IMAN_INTR_ENA)) {
+		if (pci_msi_enabled(sc->xsc_pi))
+			pci_generate_msi(sc->xsc_pi, 0);
+		else
+			pci_lintr_assert(sc->xsc_pi);
+	}
+}
+
+static void
+pci_xhci_deassert_interrupt(struct pci_xhci_softc *sc)
+{
+
+	if (!pci_msi_enabled(sc->xsc_pi))
+		pci_lintr_assert(sc->xsc_pi);
+}
+
+static void
+pci_xhci_init_ep(struct pci_xhci_dev_emu *dev, int epid)
+{
+	struct xhci_dev_ctx    *dev_ctx;
+	struct pci_xhci_dev_ep *devep;
+	struct xhci_endp_ctx   *ep_ctx;
+	uint32_t	pstreams;
+	int		i;
+
+	dev_ctx = dev->dev_ctx;
+	ep_ctx = &dev_ctx->ctx_ep[epid];
+	devep = &dev->eps[epid];
+	pstreams = XHCI_EPCTX_0_MAXP_STREAMS_GET(ep_ctx->dwEpCtx0);
+	if (pstreams > 0) {
+		DPRINTF(("init_ep %d with pstreams %d\r\n", epid, pstreams));
+		assert(devep->ep_sctx_trbs == NULL);
+
+		devep->ep_sctx = XHCI_GADDR(dev->xsc, ep_ctx->qwEpCtx2 &
+		                            XHCI_EPCTX_2_TR_DQ_PTR_MASK);
+		devep->ep_sctx_trbs = calloc(pstreams,
+		                      sizeof(struct pci_xhci_trb_ring));
+		for (i = 0; i < pstreams; i++) {
+			devep->ep_sctx_trbs[i].ringaddr =
+			                         devep->ep_sctx[i].qwSctx0 &
+			                         XHCI_SCTX_0_TR_DQ_PTR_MASK;
+			devep->ep_sctx_trbs[i].ccs =
+			     XHCI_SCTX_0_DCS_GET(devep->ep_sctx[i].qwSctx0);
+		}
+	} else {
+		DPRINTF(("init_ep %d with no pstreams\r\n", epid));
+		devep->ep_ringaddr = ep_ctx->qwEpCtx2 &
+		                     XHCI_EPCTX_2_TR_DQ_PTR_MASK;
+		devep->ep_ccs = XHCI_EPCTX_2_DCS_GET(ep_ctx->qwEpCtx2);
+		devep->ep_tr = XHCI_GADDR(dev->xsc, devep->ep_ringaddr);
+		DPRINTF(("init_ep tr DCS %x\r\n", devep->ep_ccs));
+	}
+
+	if (devep->ep_xfer == NULL) {
+		devep->ep_xfer = malloc(sizeof(struct usb_data_xfer));
+		USB_DATA_XFER_INIT(devep->ep_xfer);
+	}
+}
+
+static void
+pci_xhci_disable_ep(struct pci_xhci_dev_emu *dev, int epid)
+{
+	struct xhci_dev_ctx    *dev_ctx;
+	struct pci_xhci_dev_ep *devep;
+	struct xhci_endp_ctx   *ep_ctx;
+
+	DPRINTF(("pci_xhci disable_ep %d\r\n", epid));
+
+	dev_ctx = dev->dev_ctx;
+	ep_ctx = &dev_ctx->ctx_ep[epid];
+	ep_ctx->dwEpCtx0 = (ep_ctx->dwEpCtx0 & ~0x7) | XHCI_ST_EPCTX_DISABLED;
+
+	devep = &dev->eps[epid];
+	if (XHCI_EPCTX_0_MAXP_STREAMS_GET(ep_ctx->dwEpCtx0) > 0 &&
+	    devep->ep_sctx_trbs != NULL)
+			free(devep->ep_sctx_trbs);
+
+	if (devep->ep_xfer != NULL) {
+		free(devep->ep_xfer);
+		devep->ep_xfer = NULL;
+	}
+
+	memset(devep, 0, sizeof(struct pci_xhci_dev_ep));
+}
+
+
+/* reset device at slot and data structures related to it */
+static void
+pci_xhci_reset_slot(struct pci_xhci_softc *sc, int slot)
+{
+	struct pci_xhci_dev_emu *dev;
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+
+	if (!dev) {
+		DPRINTF(("xhci reset unassigned slot (%d)?\r\n", slot));
+	} else {
+		dev->dev_slotstate = XHCI_ST_DISABLED;
+	}
+
+	/* TODO: reset ring buffer pointers */
+}
+
+static int
+pci_xhci_insert_event(struct pci_xhci_softc *sc, struct xhci_trb *evtrb,
+    int do_intr)
+{
+	struct pci_xhci_rtsregs *rts;
+	uint64_t	erdp;
+	int		erdp_idx;
+	int		err;
+	struct xhci_trb *evtrbptr;
+
+	err = XHCI_TRB_ERROR_SUCCESS;
+
+	rts = &sc->rtsregs;
+
+	erdp = rts->intrreg.erdp & ~0xF;
+	erdp_idx = (erdp - rts->erstba_p[rts->er_deq_seg].qwEvrsTablePtr) /
+	           sizeof(struct xhci_trb);
+
+	DPRINTF(("pci_xhci: insert event 0[%lx] 2[%x] 3[%x]\r\n"
+	         "\terdp idx %d/seg %d, enq idx %d/seg %d, pcs %u\r\n"
+	         "\t(erdp=0x%lx, erst=0x%lx, tblsz=%u, do_intr %d)\r\n",
+	         evtrb->qwTrb0, evtrb->dwTrb2, evtrb->dwTrb3,
+	         erdp_idx, rts->er_deq_seg, rts->er_enq_idx,
+	         rts->er_enq_seg,
+	         rts->event_pcs, erdp, rts->erstba_p->qwEvrsTablePtr,
+	         rts->erstba_p->dwEvrsTableSize, do_intr));
+
+	evtrbptr = &rts->erst_p[rts->er_enq_idx];
+
+	/* TODO: multi-segment table */
+	if (rts->er_events_cnt >= rts->erstba_p->dwEvrsTableSize) {
+		DPRINTF(("pci_xhci[%d] cannot insert event; ring full\r\n",
+		         __LINE__));
+		err = XHCI_TRB_ERROR_EV_RING_FULL;
+		goto done;
+	}
+
+	if (rts->er_events_cnt == rts->erstba_p->dwEvrsTableSize - 1) {
+		struct xhci_trb	errev;
+
+		if ((evtrbptr->dwTrb3 & 0x1) == (rts->event_pcs & 0x1)) {
+
+			DPRINTF(("pci_xhci[%d] insert evt err: ring full\r\n",
+			         __LINE__));
+
+			errev.qwTrb0 = 0;
+			errev.dwTrb2 = XHCI_TRB_2_ERROR_SET(
+			                    XHCI_TRB_ERROR_EV_RING_FULL);
+			errev.dwTrb3 = XHCI_TRB_3_TYPE_SET(
+			                    XHCI_TRB_EVENT_HOST_CTRL) |
+			               rts->event_pcs;
+			rts->er_events_cnt++;
+			memcpy(&rts->erst_p[rts->er_enq_idx], &errev,
+			       sizeof(struct xhci_trb));
+			rts->er_enq_idx = (rts->er_enq_idx + 1) %
+			                  rts->erstba_p->dwEvrsTableSize;
+			err = XHCI_TRB_ERROR_EV_RING_FULL;
+			do_intr = 1;
+
+			goto done;
+		}
+	} else {
+		rts->er_events_cnt++;
+	}
+
+	evtrb->dwTrb3 &= ~XHCI_TRB_3_CYCLE_BIT;
+	evtrb->dwTrb3 |= rts->event_pcs;
+
+	memcpy(&rts->erst_p[rts->er_enq_idx], evtrb, sizeof(struct xhci_trb));
+	rts->er_enq_idx = (rts->er_enq_idx + 1) %
+	                  rts->erstba_p->dwEvrsTableSize;
+
+	if (rts->er_enq_idx == 0)
+		rts->event_pcs ^= 1;
+
+done:
+	if (do_intr)
+		pci_xhci_assert_interrupt(sc);
+
+	return (err);
+}
+
+static uint32_t
+pci_xhci_cmd_enable_slot(struct pci_xhci_softc *sc, uint32_t *slot)
+{
+	struct pci_xhci_dev_emu *dev;
+	uint32_t	cmderr;
+	int		i;
+
+	cmderr = XHCI_TRB_ERROR_NO_SLOTS;
+	if (sc->portregs != NULL)
+		for (i = 1; i <= XHCI_MAX_SLOTS; i++) {
+			dev = XHCI_SLOTDEV_PTR(sc, i);
+			if (dev && dev->dev_slotstate == XHCI_ST_DISABLED) {
+				*slot = i;
+				dev->dev_slotstate = XHCI_ST_ENABLED;
+				cmderr = XHCI_TRB_ERROR_SUCCESS;
+				dev->hci.hci_address = i;
+				break;
+			}
+		}
+
+	DPRINTF(("pci_xhci enable slot (error=%d) slot %u\r\n",
+		cmderr != XHCI_TRB_ERROR_SUCCESS, *slot));
+
+	return (cmderr);
+}
+
+static uint32_t
+pci_xhci_cmd_disable_slot(struct pci_xhci_softc *sc, uint32_t slot)
+{
+	struct pci_xhci_dev_emu *dev;
+	uint32_t cmderr;
+
+	DPRINTF(("pci_xhci disable slot %u\r\n", slot));
+
+	cmderr = XHCI_TRB_ERROR_NO_SLOTS;
+	if (sc->portregs == NULL)
+		goto done;
+
+	if (slot > sc->ndevices) {
+		cmderr = XHCI_TRB_ERROR_SLOT_NOT_ON;
+		goto done;
+	}
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+	if (dev) {
+		if (dev->dev_slotstate == XHCI_ST_DISABLED) {
+			cmderr = XHCI_TRB_ERROR_SLOT_NOT_ON;
+		} else {
+			dev->dev_slotstate = XHCI_ST_DISABLED;
+			cmderr = XHCI_TRB_ERROR_SUCCESS;
+			/* TODO: reset events and endpoints */
+		}
+	}
+
+done:
+	return (cmderr);
+}
+
+static uint32_t
+pci_xhci_cmd_reset_device(struct pci_xhci_softc *sc, uint32_t slot)
+{
+	struct pci_xhci_dev_emu *dev;
+	struct xhci_dev_ctx     *dev_ctx;
+	struct xhci_endp_ctx    *ep_ctx;
+	uint32_t	cmderr;
+	int		i;
+
+	cmderr = XHCI_TRB_ERROR_NO_SLOTS;
+	if (sc->portregs == NULL)
+		goto done;
+
+	DPRINTF(("pci_xhci reset device slot %u\r\n", slot));
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+	if (!dev || dev->dev_slotstate == XHCI_ST_DISABLED)
+		cmderr = XHCI_TRB_ERROR_SLOT_NOT_ON;
+	else {
+		dev->dev_slotstate = XHCI_ST_DEFAULT;
+
+		dev->hci.hci_address = 0;
+		dev_ctx = pci_xhci_get_dev_ctx(sc, slot);
+
+		/* slot state */
+		dev_ctx->ctx_slot.dwSctx3 = FIELD_REPLACE(
+		    dev_ctx->ctx_slot.dwSctx3, XHCI_ST_SLCTX_DEFAULT,
+		    0x1F, 27);
+
+		/* number of contexts */
+		dev_ctx->ctx_slot.dwSctx0 = FIELD_REPLACE(
+		    dev_ctx->ctx_slot.dwSctx0, 1, 0x1F, 27);
+
+		/* reset all eps other than ep-0 */
+		for (i = 2; i <= 31; i++) {
+			ep_ctx = &dev_ctx->ctx_ep[i];
+			ep_ctx->dwEpCtx0 = FIELD_REPLACE( ep_ctx->dwEpCtx0,
+			    XHCI_ST_EPCTX_DISABLED, 0x7, 0);
+		}
+
+		cmderr = XHCI_TRB_ERROR_SUCCESS;
+	}
+
+	pci_xhci_reset_slot(sc, slot);
+
+done:
+	return (cmderr);
+}
+
+static uint32_t
+pci_xhci_cmd_address_device(struct pci_xhci_softc *sc, uint32_t slot,
+    struct xhci_trb *trb)
+{
+	struct pci_xhci_dev_emu	*dev;
+	struct xhci_input_dev_ctx *input_ctx;
+	struct xhci_slot_ctx	*islot_ctx;
+	struct xhci_dev_ctx	*dev_ctx;
+	struct xhci_endp_ctx	*ep0_ctx;
+	uint32_t		cmderr;
+
+	input_ctx = XHCI_GADDR(sc, trb->qwTrb0 & ~0xFUL);
+	islot_ctx = &input_ctx->ctx_slot;
+	ep0_ctx = &input_ctx->ctx_ep[1];
+
+	cmderr = XHCI_TRB_ERROR_SUCCESS;
+
+	DPRINTF(("pci_xhci: address device, input ctl: D 0x%08x A 0x%08x,\r\n"
+	         "          slot %08x %08x %08x %08x\r\n"
+	         "          ep0  %08x %08x %016lx %08x\r\n",
+	        input_ctx->ctx_input.dwInCtx0, input_ctx->ctx_input.dwInCtx1,
+	        islot_ctx->dwSctx0, islot_ctx->dwSctx1,
+	        islot_ctx->dwSctx2, islot_ctx->dwSctx3,
+	        ep0_ctx->dwEpCtx0, ep0_ctx->dwEpCtx1, ep0_ctx->qwEpCtx2,
+	        ep0_ctx->dwEpCtx4));
+
+	/* when setting address: drop-ctx=0, add-ctx=slot+ep0 */
+	if ((input_ctx->ctx_input.dwInCtx0 != 0) ||
+	    (input_ctx->ctx_input.dwInCtx1 & 0x03) != 0x03) {
+		DPRINTF(("pci_xhci: address device, input ctl invalid\r\n"));
+		cmderr = XHCI_TRB_ERROR_TRB;
+		goto done;
+	}
+
+	/* assign address to slot */
+	dev_ctx = pci_xhci_get_dev_ctx(sc, slot);
+
+	DPRINTF(("pci_xhci: address device, dev ctx\r\n"
+	         "          slot %08x %08x %08x %08x\r\n",
+	        dev_ctx->ctx_slot.dwSctx0, dev_ctx->ctx_slot.dwSctx1,
+	        dev_ctx->ctx_slot.dwSctx2, dev_ctx->ctx_slot.dwSctx3));
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+	assert(dev != NULL);
+
+	dev->hci.hci_address = slot;
+	dev->dev_ctx = dev_ctx;
+
+	if (dev->dev_ue->ue_reset == NULL ||
+	    dev->dev_ue->ue_reset(dev->dev_sc) < 0) {
+		cmderr = XHCI_TRB_ERROR_ENDP_NOT_ON;
+		goto done;
+	}
+
+	memcpy(&dev_ctx->ctx_slot, islot_ctx, sizeof(struct xhci_slot_ctx));
+
+	dev_ctx->ctx_slot.dwSctx3 =
+	    XHCI_SCTX_3_SLOT_STATE_SET(XHCI_ST_SLCTX_ADDRESSED) |
+	    XHCI_SCTX_3_DEV_ADDR_SET(slot);
+
+	memcpy(&dev_ctx->ctx_ep[1], ep0_ctx, sizeof(struct xhci_endp_ctx));
+	ep0_ctx = &dev_ctx->ctx_ep[1];
+	ep0_ctx->dwEpCtx0 = (ep0_ctx->dwEpCtx0 & ~0x7) |
+	    XHCI_EPCTX_0_EPSTATE_SET(XHCI_ST_EPCTX_RUNNING);
+
+	pci_xhci_init_ep(dev, 1);
+
+	dev->dev_slotstate = XHCI_ST_ADDRESSED;
+
+	DPRINTF(("pci_xhci: address device, output ctx\r\n"
+	         "          slot %08x %08x %08x %08x\r\n"
+	         "          ep0  %08x %08x %016lx %08x\r\n",
+	        dev_ctx->ctx_slot.dwSctx0, dev_ctx->ctx_slot.dwSctx1,
+	        dev_ctx->ctx_slot.dwSctx2, dev_ctx->ctx_slot.dwSctx3,
+	        ep0_ctx->dwEpCtx0, ep0_ctx->dwEpCtx1, ep0_ctx->qwEpCtx2,
+	        ep0_ctx->dwEpCtx4));
+
+done:
+	return (cmderr);
+}
+
+static uint32_t
+pci_xhci_cmd_config_ep(struct pci_xhci_softc *sc, uint32_t slot,
+    struct xhci_trb *trb)
+{
+	struct xhci_input_dev_ctx *input_ctx;
+	struct pci_xhci_dev_emu	*dev;
+	struct xhci_dev_ctx	*dev_ctx;
+	struct xhci_endp_ctx	*ep_ctx, *iep_ctx;
+	uint32_t	cmderr;
+	int		i;
+
+	cmderr = XHCI_TRB_ERROR_SUCCESS;
+
+	DPRINTF(("pci_xhci config_ep slot %u\r\n", slot));
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+	assert(dev != NULL);
+
+	if ((trb->dwTrb3 & XHCI_TRB_3_DCEP_BIT) != 0) {
+		DPRINTF(("pci_xhci config_ep - deconfigure ep slot %u\r\n",
+		        slot));
+		if (dev->dev_ue->ue_stop != NULL)
+			dev->dev_ue->ue_stop(dev->dev_sc);
+
+		dev->dev_slotstate = XHCI_ST_ADDRESSED;
+
+		dev->hci.hci_address = 0;
+		dev_ctx = pci_xhci_get_dev_ctx(sc, slot);
+
+		/* number of contexts */
+		dev_ctx->ctx_slot.dwSctx0 = FIELD_REPLACE(
+		    dev_ctx->ctx_slot.dwSctx0, 1, 0x1F, 27);
+
+		/* slot state */
+		dev_ctx->ctx_slot.dwSctx3 = FIELD_REPLACE(
+		    dev_ctx->ctx_slot.dwSctx3, XHCI_ST_SLCTX_ADDRESSED,
+		    0x1F, 27);
+
+		/* disable endpoints */
+		for (i = 2; i < 32; i++)
+			pci_xhci_disable_ep(dev, i);
+
+		cmderr = XHCI_TRB_ERROR_SUCCESS;
+
+		goto done;
+	}
+
+	if (dev->dev_slotstate < XHCI_ST_ADDRESSED) {
+		DPRINTF(("pci_xhci: config_ep slotstate x%x != addressed\r\n",
+		        dev->dev_slotstate));
+		cmderr = XHCI_TRB_ERROR_SLOT_NOT_ON;
+		goto done;
+	}
+
+	/* In addressed/configured state;
+	 * for each drop endpoint ctx flag:
+	 *   ep->state = DISABLED
+	 * for each add endpoint ctx flag:
+	 *   cp(ep-in, ep-out)
+	 *   ep->state = RUNNING
+	 * for each drop+add endpoint flag:
+	 *   reset ep resources
+	 *   cp(ep-in, ep-out)
+	 *   ep->state = RUNNING
+	 * if input->DisabledCtx[2-31] < 30: (at least 1 ep not disabled)
+	 *   slot->state = configured
+	 */
+
+	input_ctx = XHCI_GADDR(sc, trb->qwTrb0 & ~0xFUL);
+	dev_ctx = dev->dev_ctx;
+	DPRINTF(("pci_xhci: config_ep inputctx: D:x%08x A:x%08x 7:x%08x\r\n",
+		input_ctx->ctx_input.dwInCtx0, input_ctx->ctx_input.dwInCtx1,
+	        input_ctx->ctx_input.dwInCtx7));
+
+	for (i = 2; i <= 31; i++) {
+		ep_ctx = &dev_ctx->ctx_ep[i];
+
+		if (input_ctx->ctx_input.dwInCtx0 &
+		    XHCI_INCTX_0_DROP_MASK(i)) {
+			DPRINTF((" config ep - dropping ep %d\r\n", i));
+			pci_xhci_disable_ep(dev, i);
+		}
+
+		if (input_ctx->ctx_input.dwInCtx1 &
+		    XHCI_INCTX_1_ADD_MASK(i)) {
+			iep_ctx = &input_ctx->ctx_ep[i];
+
+			DPRINTF((" enable ep[%d]  %08x %08x %016lx %08x\r\n",
+			   i, iep_ctx->dwEpCtx0, iep_ctx->dwEpCtx1,
+			   iep_ctx->qwEpCtx2, iep_ctx->dwEpCtx4));
+
+			memcpy(ep_ctx, iep_ctx, sizeof(struct xhci_endp_ctx));
+
+			pci_xhci_init_ep(dev, i);
+
+			/* ep state */
+			ep_ctx->dwEpCtx0 = FIELD_REPLACE(
+			    ep_ctx->dwEpCtx0, XHCI_ST_EPCTX_RUNNING, 0x7, 0);
+		}
+	}
+
+	/* slot state to configured */
+	dev_ctx->ctx_slot.dwSctx3 = FIELD_REPLACE(
+	    dev_ctx->ctx_slot.dwSctx3, XHCI_ST_SLCTX_CONFIGURED, 0x1F, 27);
+	dev_ctx->ctx_slot.dwSctx0 = FIELD_COPY(
+	    dev_ctx->ctx_slot.dwSctx0, input_ctx->ctx_slot.dwSctx0, 0x1F, 27);
+	dev->dev_slotstate = XHCI_ST_CONFIGURED;
+
+	DPRINTF(("EP configured; slot %u [0]=0x%08x [1]=0x%08x [2]=0x%08x "
+	         "[3]=0x%08x\r\n",
+	    slot, dev_ctx->ctx_slot.dwSctx0, dev_ctx->ctx_slot.dwSctx1,
+	    dev_ctx->ctx_slot.dwSctx2, dev_ctx->ctx_slot.dwSctx3));
+
+done:
+	return (cmderr);
+}
+
+static uint32_t
+pci_xhci_cmd_reset_ep(struct pci_xhci_softc *sc, uint32_t slot,
+    struct xhci_trb *trb)
+{
+	struct pci_xhci_dev_emu	*dev;
+	struct pci_xhci_dev_ep *devep;
+	struct xhci_dev_ctx	*dev_ctx;
+	struct xhci_endp_ctx	*ep_ctx;
+	uint32_t	cmderr, epid;
+	uint32_t	type;
+
+	epid = XHCI_TRB_3_EP_GET(trb->dwTrb3);
+
+	DPRINTF(("pci_xhci: reset ep %u: slot %u\r\n", epid, slot));
+
+	cmderr = XHCI_TRB_ERROR_SUCCESS;
+
+	type = XHCI_TRB_3_TYPE_GET(trb->dwTrb3);
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+	assert(dev != NULL);
+
+	if (type == XHCI_TRB_TYPE_STOP_EP &&
+	    (trb->dwTrb3 & XHCI_TRB_3_SUSP_EP_BIT) != 0) {
+		/* XXX suspend endpoint for 10ms */
+	}
+
+	if (epid < 1 || epid > 31) {
+		DPRINTF(("pci_xhci: reset ep: invalid epid %u\r\n", epid));
+		cmderr = XHCI_TRB_ERROR_TRB;
+		goto done;
+	}
+
+	devep = &dev->eps[epid];
+	if (devep->ep_xfer != NULL)
+		USB_DATA_XFER_RESET(devep->ep_xfer);
+
+	dev_ctx = dev->dev_ctx;
+	assert(dev_ctx != NULL);
+
+	ep_ctx = &dev_ctx->ctx_ep[epid];
+
+	ep_ctx->dwEpCtx0 = (ep_ctx->dwEpCtx0 & ~0x7) | XHCI_ST_EPCTX_STOPPED;
+
+	if (XHCI_EPCTX_0_MAXP_STREAMS_GET(ep_ctx->dwEpCtx0) == 0)
+		ep_ctx->qwEpCtx2 = devep->ep_ringaddr | devep->ep_ccs;
+
+	DPRINTF(("pci_xhci: reset ep[%u] %08x %08x %016lx %08x\r\n",
+	        epid, ep_ctx->dwEpCtx0, ep_ctx->dwEpCtx1, ep_ctx->qwEpCtx2,
+	        ep_ctx->dwEpCtx4));
+
+	if (type == XHCI_TRB_TYPE_RESET_EP &&
+	    (dev->dev_ue->ue_reset == NULL ||
+	    dev->dev_ue->ue_reset(dev->dev_sc) < 0)) {
+		cmderr = XHCI_TRB_ERROR_ENDP_NOT_ON;
+		goto done;
+	}
+
+done:
+	return (cmderr);
+}
+
+
+static uint32_t
+pci_xhci_find_stream(struct pci_xhci_softc *sc, struct xhci_endp_ctx *ep,
+    uint32_t streamid, struct xhci_stream_ctx **osctx)
+{
+	struct xhci_stream_ctx *sctx;
+	uint32_t	maxpstreams;
+
+	maxpstreams = XHCI_EPCTX_0_MAXP_STREAMS_GET(ep->dwEpCtx0);
+	if (maxpstreams == 0)
+		return (XHCI_TRB_ERROR_TRB);
+
+	if (maxpstreams > XHCI_STREAMS_MAX)
+		return (XHCI_TRB_ERROR_INVALID_SID);
+
+	if (XHCI_EPCTX_0_LSA_GET(ep->dwEpCtx0) == 0) {
+		DPRINTF(("pci_xhci: find_stream; LSA bit not set\r\n"));
+		return (XHCI_TRB_ERROR_INVALID_SID);
+	}
+
+	/* only support primary stream */
+	if (streamid > maxpstreams)
+		return (XHCI_TRB_ERROR_STREAM_TYPE);
+
+	sctx = XHCI_GADDR(sc, ep->qwEpCtx2 & ~0xFUL) + streamid;
+	if (!XHCI_SCTX_0_SCT_GET(sctx->qwSctx0))
+		return (XHCI_TRB_ERROR_STREAM_TYPE);
+
+	*osctx = sctx;
+
+	return (XHCI_TRB_ERROR_SUCCESS);
+}
+
+
+static uint32_t
+pci_xhci_cmd_set_tr(struct pci_xhci_softc *sc, uint32_t slot,
+    struct xhci_trb *trb)
+{
+	struct pci_xhci_dev_emu	*dev;
+	struct pci_xhci_dev_ep	*devep;
+	struct xhci_dev_ctx	*dev_ctx;
+	struct xhci_endp_ctx	*ep_ctx;
+	uint32_t	cmderr, epid;
+	uint32_t	streamid;
+
+	cmderr = XHCI_TRB_ERROR_SUCCESS;
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+	assert(dev != NULL);
+
+	DPRINTF(("pci_xhci set_tr: new-tr x%016lx, SCT %u DCS %u\r\n"
+	         "                 stream-id %u, slot %u, epid %u, C %u\r\n",
+	         (trb->qwTrb0 & ~0xF),  (uint32_t)((trb->qwTrb0 >> 1) & 0x7),
+	         (uint32_t)(trb->qwTrb0 & 0x1), (trb->dwTrb2 >> 16) & 0xFFFF,
+	         XHCI_TRB_3_SLOT_GET(trb->dwTrb3),
+	         XHCI_TRB_3_EP_GET(trb->dwTrb3), trb->dwTrb3 & 0x1));
+
+	epid = XHCI_TRB_3_EP_GET(trb->dwTrb3);
+	if (epid < 1 || epid > 31) {
+		DPRINTF(("pci_xhci: set_tr_deq: invalid epid %u\r\n", epid));
+		cmderr = XHCI_TRB_ERROR_TRB;
+		goto done;
+	}
+
+	dev_ctx = dev->dev_ctx;
+	assert(dev_ctx != NULL);
+
+	ep_ctx = &dev_ctx->ctx_ep[epid];
+	devep = &dev->eps[epid];
+
+	switch (XHCI_EPCTX_0_EPSTATE_GET(ep_ctx->dwEpCtx0)) {
+	case XHCI_ST_EPCTX_STOPPED:
+	case XHCI_ST_EPCTX_ERROR:
+		break;
+	default:
+		DPRINTF(("pci_xhci cmd set_tr invalid state %x\r\n",
+		        XHCI_EPCTX_0_EPSTATE_GET(ep_ctx->dwEpCtx0)));
+		cmderr = XHCI_TRB_ERROR_CONTEXT_STATE;
+		goto done;
+	}
+
+	streamid = XHCI_TRB_2_STREAM_GET(trb->dwTrb2);
+	if (XHCI_EPCTX_0_MAXP_STREAMS_GET(ep_ctx->dwEpCtx0) > 0) {
+		struct xhci_stream_ctx *sctx;
+
+		sctx = NULL;
+		cmderr = pci_xhci_find_stream(sc, ep_ctx, streamid, &sctx);
+		if (sctx != NULL) {
+			assert(devep->ep_sctx != NULL);
+			
+			devep->ep_sctx[streamid].qwSctx0 = trb->qwTrb0;
+			devep->ep_sctx_trbs[streamid].ringaddr =
+			    trb->qwTrb0 & ~0xF;
+			devep->ep_sctx_trbs[streamid].ccs =
+			    XHCI_EPCTX_2_DCS_GET(trb->qwTrb0);
+		}
+	} else {
+		if (streamid != 0) {
+			DPRINTF(("pci_xhci cmd set_tr streamid %x != 0\r\n",
+			        streamid));
+		}
+		ep_ctx->qwEpCtx2 = trb->qwTrb0 & ~0xFUL;
+		devep->ep_ringaddr = ep_ctx->qwEpCtx2 & ~0xFUL;
+		devep->ep_ccs = trb->qwTrb0 & 0x1;
+		devep->ep_tr = XHCI_GADDR(sc, devep->ep_ringaddr);
+
+		DPRINTF(("pci_xhci set_tr first TRB:\r\n"));
+		pci_xhci_dump_trb(devep->ep_tr);
+	}
+	ep_ctx->dwEpCtx0 = (ep_ctx->dwEpCtx0 & ~0x7) | XHCI_ST_EPCTX_STOPPED;
+
+done:
+	return (cmderr);
+}
+
+static uint32_t
+pci_xhci_cmd_eval_ctx(struct pci_xhci_softc *sc, uint32_t slot,
+    struct xhci_trb *trb)
+{
+	struct xhci_input_dev_ctx *input_ctx;
+	struct xhci_slot_ctx      *islot_ctx;
+	struct xhci_dev_ctx       *dev_ctx;
+	struct xhci_endp_ctx      *ep0_ctx;
+	uint32_t cmderr;
+
+	input_ctx = XHCI_GADDR(sc, trb->qwTrb0 & ~0xFUL);
+	islot_ctx = &input_ctx->ctx_slot;
+	ep0_ctx = &input_ctx->ctx_ep[1];
+
+	cmderr = XHCI_TRB_ERROR_SUCCESS;
+	DPRINTF(("pci_xhci: eval ctx, input ctl: D 0x%08x A 0x%08x,\r\n"
+	         "          slot %08x %08x %08x %08x\r\n"
+	         "          ep0  %08x %08x %016lx %08x\r\n",
+	        input_ctx->ctx_input.dwInCtx0, input_ctx->ctx_input.dwInCtx1,
+	        islot_ctx->dwSctx0, islot_ctx->dwSctx1,
+	        islot_ctx->dwSctx2, islot_ctx->dwSctx3,
+	        ep0_ctx->dwEpCtx0, ep0_ctx->dwEpCtx1, ep0_ctx->qwEpCtx2,
+	        ep0_ctx->dwEpCtx4));
+
+	/* this command expects drop-ctx=0 & add-ctx=slot+ep0 */
+	if ((input_ctx->ctx_input.dwInCtx0 != 0) ||
+	    (input_ctx->ctx_input.dwInCtx1 & 0x03) == 0) {
+		DPRINTF(("pci_xhci: eval ctx, input ctl invalid\r\n"));
+		cmderr = XHCI_TRB_ERROR_TRB;
+		goto done;
+	}
+
+	/* assign address to slot; in this emulation, slot_id = address */
+	dev_ctx = pci_xhci_get_dev_ctx(sc, slot);
+
+	DPRINTF(("pci_xhci: eval ctx, dev ctx\r\n"
+	         "          slot %08x %08x %08x %08x\r\n",
+	        dev_ctx->ctx_slot.dwSctx0, dev_ctx->ctx_slot.dwSctx1,
+	        dev_ctx->ctx_slot.dwSctx2, dev_ctx->ctx_slot.dwSctx3));
+
+	if (input_ctx->ctx_input.dwInCtx1 & 0x01) {	/* slot ctx */
+		/* set max exit latency */
+		dev_ctx->ctx_slot.dwSctx1 = FIELD_COPY(
+		    dev_ctx->ctx_slot.dwSctx1, input_ctx->ctx_slot.dwSctx1,
+		    0xFFFF, 0);
+
+		/* set interrupter target */
+		dev_ctx->ctx_slot.dwSctx2 = FIELD_COPY(
+		    dev_ctx->ctx_slot.dwSctx2, input_ctx->ctx_slot.dwSctx2,
+		    0x3FF, 22);
+	}
+	if (input_ctx->ctx_input.dwInCtx1 & 0x02) {	/* control ctx */
+		/* set max packet size */
+		dev_ctx->ctx_ep[1].dwEpCtx1 = FIELD_COPY(
+		    dev_ctx->ctx_ep[1].dwEpCtx1, ep0_ctx->dwEpCtx1,
+		    0xFFFF, 16);
+
+		ep0_ctx = &dev_ctx->ctx_ep[1];
+	}
+
+	DPRINTF(("pci_xhci: eval ctx, output ctx\r\n"
+	         "          slot %08x %08x %08x %08x\r\n"
+	         "          ep0  %08x %08x %016lx %08x\r\n",
+	        dev_ctx->ctx_slot.dwSctx0, dev_ctx->ctx_slot.dwSctx1,
+	        dev_ctx->ctx_slot.dwSctx2, dev_ctx->ctx_slot.dwSctx3,
+	        ep0_ctx->dwEpCtx0, ep0_ctx->dwEpCtx1, ep0_ctx->qwEpCtx2,
+	        ep0_ctx->dwEpCtx4));
+
+done:
+	return (cmderr);
+}
+
+static int
+pci_xhci_complete_commands(struct pci_xhci_softc *sc)
+{
+	struct xhci_trb	evtrb;
+	struct xhci_trb	*trb;
+	uint64_t	crcr;
+	uint32_t	ccs;		/* cycle state (XHCI 4.9.2) */
+	uint32_t	type;
+	uint32_t	slot;
+	uint32_t	cmderr;
+	int		error;
+
+	error = 0;
+	sc->opregs.crcr |= XHCI_CRCR_LO_CRR;
+
+	trb = sc->opregs.cr_p;
+	ccs = sc->opregs.crcr & XHCI_CRCR_LO_RCS;
+	crcr = sc->opregs.crcr & ~0xF;
+
+	while (1) {
+		sc->opregs.cr_p = trb;
+	
+		type = XHCI_TRB_3_TYPE_GET(trb->dwTrb3);
+
+		if ((trb->dwTrb3 & XHCI_TRB_3_CYCLE_BIT) !=
+		    (ccs & XHCI_TRB_3_CYCLE_BIT))
+			break;
+
+		DPRINTF(("pci_xhci: cmd type 0x%x, Trb0 x%016lx dwTrb2 x%08x"
+		        " dwTrb3 x%08x, TRB_CYCLE %u/ccs %u\r\n",
+		        type, trb->qwTrb0, trb->dwTrb2, trb->dwTrb3,
+		        trb->dwTrb3 & XHCI_TRB_3_CYCLE_BIT, ccs));
+
+		cmderr = XHCI_TRB_ERROR_SUCCESS;
+		evtrb.dwTrb2 = 0;
+		evtrb.dwTrb3 = (ccs & XHCI_TRB_3_CYCLE_BIT) |
+		      XHCI_TRB_3_TYPE_SET(XHCI_TRB_EVENT_CMD_COMPLETE);
+		slot = 0;
+
+		switch (type) {
+		case XHCI_TRB_TYPE_LINK:			/* 0x06 */
+			if (trb->dwTrb3 & XHCI_TRB_3_TC_BIT)
+				ccs ^= XHCI_CRCR_LO_RCS;
+			break;
+
+		case XHCI_TRB_TYPE_ENABLE_SLOT:			/* 0x09 */
+			cmderr = pci_xhci_cmd_enable_slot(sc, &slot);
+			break;
+
+		case XHCI_TRB_TYPE_DISABLE_SLOT:		/* 0x0A */
+			slot = XHCI_TRB_3_SLOT_GET(trb->dwTrb3);
+			cmderr = pci_xhci_cmd_disable_slot(sc, slot);
+			break;
+
+		case XHCI_TRB_TYPE_ADDRESS_DEVICE:		/* 0x0B */
+			slot = XHCI_TRB_3_SLOT_GET(trb->dwTrb3);
+			cmderr = pci_xhci_cmd_address_device(sc, slot, trb);
+			break;
+
+		case XHCI_TRB_TYPE_CONFIGURE_EP:		/* 0x0C */
+			slot = XHCI_TRB_3_SLOT_GET(trb->dwTrb3);
+			cmderr = pci_xhci_cmd_config_ep(sc, slot, trb);
+			break;
+
+		case XHCI_TRB_TYPE_EVALUATE_CTX:		/* 0x0D */
+			slot = XHCI_TRB_3_SLOT_GET(trb->dwTrb3);
+			cmderr = pci_xhci_cmd_eval_ctx(sc, slot, trb);
+			break;
+
+		case XHCI_TRB_TYPE_RESET_EP:			/* 0x0E */
+			DPRINTF(("Reset Endpoint on slot %d\r\n", slot));
+			slot = XHCI_TRB_3_SLOT_GET(trb->dwTrb3);
+			cmderr = pci_xhci_cmd_reset_ep(sc, slot, trb);
+			break;
+
+		case XHCI_TRB_TYPE_STOP_EP:			/* 0x0F */
+			DPRINTF(("Stop Endpoint on slot %d\r\n", slot));
+			slot = XHCI_TRB_3_SLOT_GET(trb->dwTrb3);
+			cmderr = pci_xhci_cmd_reset_ep(sc, slot, trb);
+			break;
+
+		case XHCI_TRB_TYPE_SET_TR_DEQUEUE:		/* 0x10 */
+			slot = XHCI_TRB_3_SLOT_GET(trb->dwTrb3);
+			cmderr = pci_xhci_cmd_set_tr(sc, slot, trb);
+			break;
+
+		case XHCI_TRB_TYPE_RESET_DEVICE:		/* 0x11 */
+			slot = XHCI_TRB_3_SLOT_GET(trb->dwTrb3);
+			cmderr = pci_xhci_cmd_reset_device(sc, slot);
+			break;
+
+		case XHCI_TRB_TYPE_FORCE_EVENT:			/* 0x12 */
+			/* TODO: */
+			break;
+
+		case XHCI_TRB_TYPE_NEGOTIATE_BW:		/* 0x13 */
+			break;
+
+		case XHCI_TRB_TYPE_SET_LATENCY_TOL:		/* 0x14 */
+			break;
+
+		case XHCI_TRB_TYPE_GET_PORT_BW:			/* 0x15 */
+			break;
+
+		case XHCI_TRB_TYPE_FORCE_HEADER:		/* 0x16 */
+			break;
+
+		case XHCI_TRB_TYPE_NOOP_CMD:			/* 0x17 */
+			break;
+
+		default:
+			DPRINTF(("pci_xhci: unsupported cmd %x\r\n", type));
+			break;
+		}
+
+		if (type != XHCI_TRB_TYPE_LINK) {
+			/* 
+			 * insert command completion event and assert intr
+			 */
+			evtrb.qwTrb0 = crcr;
+			evtrb.dwTrb2 |= XHCI_TRB_2_ERROR_SET(cmderr);
+			evtrb.dwTrb3 |= XHCI_TRB_3_SLOT_SET(slot);
+			DPRINTF(("pci_xhci: command 0x%x result: 0x%x\r\n",
+			        type, cmderr));
+			pci_xhci_insert_event(sc, &evtrb, 1);
+		}
+
+		trb = pci_xhci_trb_next(sc, trb, &crcr);
+	}
+
+	sc->opregs.crcr = crcr | (sc->opregs.crcr & XHCI_CRCR_LO_CA) | ccs;
+	sc->opregs.crcr &= ~XHCI_CRCR_LO_CRR;
+	return (error);
+}
+
+static void
+pci_xhci_dump_trb(struct xhci_trb *trb)
+{
+	static const char *trbtypes[] = {
+		"RESERVED",
+		"NORMAL",
+		"SETUP_STAGE",
+		"DATA_STAGE",
+		"STATUS_STAGE",
+		"ISOCH",
+		"LINK",
+		"EVENT_DATA",
+		"NOOP",
+		"ENABLE_SLOT",
+		"DISABLE_SLOT",
+		"ADDRESS_DEVICE",
+		"CONFIGURE_EP",
+		"EVALUATE_CTX",
+		"RESET_EP",
+		"STOP_EP",
+		"SET_TR_DEQUEUE",
+		"RESET_DEVICE",
+		"FORCE_EVENT",
+		"NEGOTIATE_BW",
+		"SET_LATENCY_TOL",
+		"GET_PORT_BW",
+		"FORCE_HEADER",
+		"NOOP_CMD"
+	};
+	uint32_t type;
+
+	type = XHCI_TRB_3_TYPE_GET(trb->dwTrb3);
+	DPRINTF(("pci_xhci: trb[@%p] type x%02x %s 0:x%016lx 2:x%08x 3:x%08x\r\n",
+	         trb, type,
+	         type <= XHCI_TRB_TYPE_NOOP_CMD ? trbtypes[type] : "INVALID",
+	         trb->qwTrb0, trb->dwTrb2, trb->dwTrb3));
+}
+
+static int
+pci_xhci_xfer_complete(struct pci_xhci_softc *sc, struct usb_data_xfer *xfer,
+     uint32_t slot, uint32_t epid, int *do_intr)
+{
+	struct pci_xhci_dev_emu *dev;
+	struct pci_xhci_dev_ep	*devep;
+	struct xhci_dev_ctx	*dev_ctx;
+	struct xhci_endp_ctx	*ep_ctx;
+	struct xhci_trb		*trb;
+	struct xhci_trb		evtrb;
+	uint32_t trbflags;
+	uint32_t edtla;
+	int i, err;
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+	devep = &dev->eps[epid];
+	dev_ctx = pci_xhci_get_dev_ctx(sc, slot);
+
+	assert(dev_ctx != NULL);
+
+	ep_ctx = &dev_ctx->ctx_ep[epid];
+
+	err = XHCI_TRB_ERROR_SUCCESS;
+	*do_intr = 0;
+	edtla = 0;
+
+	/* go through list of TRBs and insert event(s) */
+	for (i = xfer->head; xfer->ndata > 0; ) {
+		evtrb.qwTrb0 = (uint64_t)xfer->data[i].hci_data;
+		trb = XHCI_GADDR(sc, evtrb.qwTrb0);
+		trbflags = trb->dwTrb3;
+
+		DPRINTF(("pci_xhci: xfer[%d] done?%u:%d trb %x %016lx %x "
+		         "(err %d) IOC?%d\r\n",
+		     i, xfer->data[i].processed, xfer->data[i].blen,
+		     XHCI_TRB_3_TYPE_GET(trbflags), evtrb.qwTrb0,
+		     trbflags, err,
+		     trb->dwTrb3 & XHCI_TRB_3_IOC_BIT ? 1 : 0));
+
+		if (!xfer->data[i].processed) {
+			xfer->head = i;
+			break;
+		}
+
+		xfer->ndata--;
+		edtla += xfer->data[i].bdone;
+
+		trb->dwTrb3 = (trb->dwTrb3 & ~0x1) | (xfer->data[i].ccs);
+
+		pci_xhci_update_ep_ring(sc, dev, devep, ep_ctx,
+		    xfer->data[i].streamid, xfer->data[i].trbnext,
+		    xfer->data[i].ccs);
+
+		/* Only interrupt if IOC or short packet */
+		if (!(trb->dwTrb3 & XHCI_TRB_3_IOC_BIT) &&
+		    !((err == XHCI_TRB_ERROR_SHORT_PKT) &&
+		      (trb->dwTrb3 & XHCI_TRB_3_ISP_BIT))) {
+
+			i = (i + 1) % USB_MAX_XFER_BLOCKS;
+			continue;
+		}
+
+		evtrb.dwTrb2 = XHCI_TRB_2_ERROR_SET(err) |
+		               XHCI_TRB_2_REM_SET(xfer->data[i].blen);
+
+		evtrb.dwTrb3 = XHCI_TRB_3_TYPE_SET(XHCI_TRB_EVENT_TRANSFER) |
+		    XHCI_TRB_3_SLOT_SET(slot) | XHCI_TRB_3_EP_SET(epid);
+
+		if (XHCI_TRB_3_TYPE_GET(trbflags) == XHCI_TRB_TYPE_EVENT_DATA) {
+			DPRINTF(("pci_xhci EVENT_DATA edtla %u\r\n", edtla));
+			evtrb.qwTrb0 = trb->qwTrb0;
+			evtrb.dwTrb2 = (edtla & 0xFFFFF) | 
+			         XHCI_TRB_2_ERROR_SET(err);
+			evtrb.dwTrb3 |= XHCI_TRB_3_ED_BIT;
+			edtla = 0;
+		}
+
+		*do_intr = 1;
+
+		err = pci_xhci_insert_event(sc, &evtrb, 0);
+		if (err != XHCI_TRB_ERROR_SUCCESS) {
+			break;
+		}
+
+		i = (i + 1) % USB_MAX_XFER_BLOCKS;
+	}
+
+	return (err);
+}
+
+static void
+pci_xhci_update_ep_ring(struct pci_xhci_softc *sc, struct pci_xhci_dev_emu *dev,
+    struct pci_xhci_dev_ep *devep, struct xhci_endp_ctx *ep_ctx,
+    uint32_t streamid, uint64_t ringaddr, int ccs)
+{
+
+	if (XHCI_EPCTX_0_MAXP_STREAMS_GET(ep_ctx->dwEpCtx0) != 0) {
+		devep->ep_sctx[streamid].qwSctx0 = (ringaddr & ~0xFUL) |
+		                                   (ccs & 0x1);
+
+		devep->ep_sctx_trbs[streamid].ringaddr = ringaddr & ~0xFUL;
+		devep->ep_sctx_trbs[streamid].ccs = ccs & 0x1;
+		ep_ctx->qwEpCtx2 = (ep_ctx->qwEpCtx2 & ~0x1) | (ccs & 0x1);
+
+		DPRINTF(("xhci update ep-ring stream %d, addr %lx\r\n",
+		    streamid, devep->ep_sctx[streamid].qwSctx0));
+	} else {
+		devep->ep_ringaddr = ringaddr & ~0xFUL;
+		devep->ep_ccs = ccs & 0x1;
+		devep->ep_tr = XHCI_GADDR(sc, ringaddr & ~0xFUL);
+		ep_ctx->qwEpCtx2 = (ringaddr & ~0xFUL) | (ccs & 0x1);
+
+		DPRINTF(("xhci update ep-ring, addr %lx\r\n",
+		    (devep->ep_ringaddr | devep->ep_ccs)));
+	}
+}
+
+/*
+ * Outstanding transfer still in progress (device NAK'd earlier) so retry
+ * the transfer again to see if it succeeds.
+ */
+static int
+pci_xhci_try_usb_xfer(struct pci_xhci_softc *sc,
+    struct pci_xhci_dev_emu *dev, struct pci_xhci_dev_ep *devep,
+    struct xhci_endp_ctx *ep_ctx, uint32_t slot, uint32_t epid)
+{
+	struct usb_data_xfer *xfer;
+	int		err;
+	int		do_intr;
+
+	ep_ctx->dwEpCtx0 = FIELD_REPLACE(
+		    ep_ctx->dwEpCtx0, XHCI_ST_EPCTX_RUNNING, 0x7, 0);
+
+	err = 0;
+	do_intr = 0;
+
+	xfer = devep->ep_xfer;
+	USB_DATA_XFER_LOCK(xfer);
+
+	/* outstanding requests queued up */
+	if (dev->dev_ue->ue_data != NULL) {
+		err = dev->dev_ue->ue_data(dev->dev_sc, xfer,
+		            epid & 0x1 ? USB_XFER_IN : USB_XFER_OUT, epid/2);
+		if (err == USB_ERR_CANCELLED) {
+			if (USB_DATA_GET_ERRCODE(&xfer->data[xfer->head]) ==
+			    USB_NAK)
+				err = XHCI_TRB_ERROR_SUCCESS;
+		} else {
+			err = pci_xhci_xfer_complete(sc, xfer, slot, epid,
+			                             &do_intr);
+			if (err == XHCI_TRB_ERROR_SUCCESS && do_intr) {
+				pci_xhci_assert_interrupt(sc);
+			}
+
+
+			/* XXX should not do it if error? */
+			USB_DATA_XFER_RESET(xfer);
+		}
+	}
+
+	USB_DATA_XFER_UNLOCK(xfer);
+
+
+	return (err);
+}
+
+
+static int
+pci_xhci_handle_transfer(struct pci_xhci_softc *sc,
+    struct pci_xhci_dev_emu *dev, struct pci_xhci_dev_ep *devep,
+    struct xhci_endp_ctx *ep_ctx, struct xhci_trb *trb, uint32_t slot,
+    uint32_t epid, uint64_t addr, uint32_t ccs, uint32_t streamid)
+{
+	struct xhci_trb *setup_trb;
+	struct usb_data_xfer *xfer;
+	struct usb_data_xfer_block *xfer_block;
+	uint64_t	val, setup_addr, status_addr;
+	uint32_t	trbflags;
+	int		do_intr, err;
+	int		do_retry;
+
+	ep_ctx->dwEpCtx0 = FIELD_REPLACE(ep_ctx->dwEpCtx0,
+	                                 XHCI_ST_EPCTX_RUNNING, 0x7, 0);
+
+	xfer = devep->ep_xfer;
+	USB_DATA_XFER_LOCK(xfer);
+
+	DPRINTF(("pci_xhci handle_transfer slot %u\r\n", slot));
+
+retry:
+	err = 0;
+	do_retry = 0;
+	do_intr = 0;
+	setup_trb = NULL;
+	setup_addr = 0;
+	status_addr = 0;
+
+	while (1) {
+		pci_xhci_dump_trb(trb);
+
+		trbflags = trb->dwTrb3;
+
+		if (XHCI_TRB_3_TYPE_GET(trbflags) != XHCI_TRB_TYPE_LINK &&
+		    (trbflags & XHCI_TRB_3_CYCLE_BIT) !=
+		    (ccs & XHCI_TRB_3_CYCLE_BIT)) {
+			DPRINTF(("Cycle-bit changed trbflags %x, ccs %x\r\n",
+			    trbflags & XHCI_TRB_3_CYCLE_BIT, ccs));
+			break;
+		}
+
+		xfer_block = NULL;
+
+		switch (XHCI_TRB_3_TYPE_GET(trbflags)) {
+		case XHCI_TRB_TYPE_LINK:
+			if (trb->dwTrb3 & XHCI_TRB_3_TC_BIT)
+				ccs ^= 0x1;
+
+			xfer_block = usb_data_xfer_append(xfer, NULL, 0,
+			                                  (void *)addr, ccs);
+			xfer_block->processed = 1;
+			break;
+
+		case XHCI_TRB_TYPE_SETUP_STAGE:
+			if ((trbflags & XHCI_TRB_3_IDT_BIT) == 0 ||
+			    XHCI_TRB_2_BYTES_GET(trb->dwTrb2) != 8) {
+				DPRINTF(("pci_xhci: invalid setup trb\r\n"));
+				err = XHCI_TRB_ERROR_TRB;
+				goto errout;
+			}
+			setup_trb = trb;
+			setup_addr = addr;
+
+			val = trb->qwTrb0;
+			if (!xfer->ureq)
+				xfer->ureq = malloc(
+				           sizeof(struct usb_device_request));
+			memcpy(xfer->ureq, &val,
+			       sizeof(struct usb_device_request));
+
+			xfer_block = usb_data_xfer_append(xfer, NULL, 0,
+			                                  (void *)addr, ccs);
+			xfer_block->processed = 1;
+			break;
+
+		case XHCI_TRB_TYPE_NORMAL:
+		case XHCI_TRB_TYPE_ISOCH:
+			if (setup_trb != NULL) {
+				DPRINTF(("pci_xhci: trb not supposed to be in "
+				         "ctl scope\r\n"));
+				err = XHCI_TRB_ERROR_TRB;
+				goto errout;
+			}
+			/* fall through */
+
+		case XHCI_TRB_TYPE_DATA_STAGE:
+			xfer_block = usb_data_xfer_append(xfer,
+			     (void *)(trbflags & XHCI_TRB_3_IDT_BIT ?
+			         &trb->qwTrb0 : XHCI_GADDR(sc, trb->qwTrb0)),
+			     trb->dwTrb2 & 0x1FFFF, (void *)addr, ccs);
+			break;
+
+		case XHCI_TRB_TYPE_STATUS_STAGE:
+			status_addr = addr;
+			xfer_block = usb_data_xfer_append(xfer, NULL, 0,
+			                                  (void *)addr, ccs);
+			break;
+
+		case XHCI_TRB_TYPE_NOOP:
+			xfer_block = usb_data_xfer_append(xfer, NULL, 0,
+			                                  (void *)addr, ccs);
+			xfer_block->processed = 1;
+			break;
+
+		case XHCI_TRB_TYPE_EVENT_DATA:
+			xfer_block = usb_data_xfer_append(xfer, NULL, 0,
+			                                  (void *)addr, ccs);
+			if ((epid > 1) && (trbflags & XHCI_TRB_3_IOC_BIT)) {
+				xfer_block->processed = 1;
+			}
+			break;
+
+		default:
+			DPRINTF(("pci_xhci: handle xfer unexpected trb type "
+			         "0x%x\r\n",
+			         XHCI_TRB_3_TYPE_GET(trbflags)));
+			err = XHCI_TRB_ERROR_TRB;
+			goto errout;
+		}
+
+		trb = pci_xhci_trb_next(sc, trb, &addr);
+
+		DPRINTF(("pci_xhci: next trb: 0x%lx\r\n", (uint64_t)trb));
+
+		if (xfer_block) {
+			xfer_block->trbnext = addr;
+			xfer_block->streamid = streamid;
+		}
+
+		if (!setup_trb && !(trbflags & XHCI_TRB_3_CHAIN_BIT) &&
+		    XHCI_TRB_3_TYPE_GET(trbflags) != XHCI_TRB_TYPE_LINK) {
+			break;
+		}
+
+		/* handle current batch that requires interrupt on complete */
+		if (trbflags & XHCI_TRB_3_IOC_BIT) {
+			DPRINTF(("pci_xhci: trb IOC bit set\r\n"));
+			if (epid == 1)
+				do_retry = 1;
+			break;
+		}
+	}
+
+	DPRINTF(("pci_xhci[%d]: xfer->ndata %u\r\n", __LINE__, xfer->ndata));
+
+	if (epid == 1) {
+		err = USB_ERR_NOT_STARTED;
+		if (dev->dev_ue->ue_request != NULL)
+			err = dev->dev_ue->ue_request(dev->dev_sc, xfer);
+		status_addr = 0;
+		setup_trb = NULL;
+	} else {
+		/* handle data transfer */
+		pci_xhci_try_usb_xfer(sc, dev, devep, ep_ctx, slot, epid);
+		err = XHCI_TRB_ERROR_SUCCESS;
+		goto errout;
+	}
+
+	err = USB_TO_XHCI_ERR(err);
+	if ((err == XHCI_TRB_ERROR_SUCCESS) ||
+	    (err == XHCI_TRB_ERROR_SHORT_PKT)) {
+		err = pci_xhci_xfer_complete(sc, xfer, slot, epid, &do_intr);
+		if (err != XHCI_TRB_ERROR_SUCCESS)
+			do_retry = 0;
+	}
+
+errout:
+	if (err == XHCI_TRB_ERROR_EV_RING_FULL)
+		DPRINTF(("pci_xhci[%d]: event ring full\r\n", __LINE__));
+
+	if (!do_retry)
+		USB_DATA_XFER_UNLOCK(xfer);
+
+	if (do_intr)
+		pci_xhci_assert_interrupt(sc);
+
+	if (do_retry) {
+		USB_DATA_XFER_RESET(xfer);
+		DPRINTF(("pci_xhci[%d]: retry:continuing with next TRBs\r\n",
+		         __LINE__));
+		goto retry;
+	}
+
+	if (epid == 1)
+		USB_DATA_XFER_RESET(xfer);
+
+	return (err);
+}
+
+static void
+pci_xhci_device_doorbell(struct pci_xhci_softc *sc, uint32_t slot,
+    uint32_t epid, uint32_t streamid)
+{
+	struct pci_xhci_dev_emu *dev;
+	struct pci_xhci_dev_ep	*devep;
+	struct xhci_dev_ctx	*dev_ctx;
+	struct xhci_endp_ctx	*ep_ctx;
+	struct pci_xhci_trb_ring *sctx_tr;
+	struct xhci_trb	*trb;
+	uint64_t	ringaddr;
+	uint32_t	ccs;
+
+	DPRINTF(("pci_xhci doorbell slot %u epid %u stream %u\r\n",
+	    slot, epid, streamid));
+
+	if (slot == 0 || slot > sc->ndevices) {
+		DPRINTF(("pci_xhci: invalid doorbell slot %u\r\n", slot));
+		return;
+	}
+
+	dev = XHCI_SLOTDEV_PTR(sc, slot);
+	devep = &dev->eps[epid];
+	dev_ctx = pci_xhci_get_dev_ctx(sc, slot);
+	if (!dev_ctx) {
+		return;
+	}
+	ep_ctx = &dev_ctx->ctx_ep[epid];
+
+	sctx_tr = NULL;
+
+	DPRINTF(("pci_xhci: device doorbell ep[%u] %08x %08x %016lx %08x\r\n",
+	        epid, ep_ctx->dwEpCtx0, ep_ctx->dwEpCtx1, ep_ctx->qwEpCtx2,
+	        ep_ctx->dwEpCtx4));
+
+	if (ep_ctx->qwEpCtx2 == 0)
+		return;
+
+	/* handle pending transfers */
+	if (devep->ep_xfer->ndata > 0) {
+		pci_xhci_try_usb_xfer(sc, dev, devep, ep_ctx, slot, epid);
+		return;
+	}
+
+	/* get next trb work item */
+	if (XHCI_EPCTX_0_MAXP_STREAMS_GET(ep_ctx->dwEpCtx0) != 0) {
+		sctx_tr = &devep->ep_sctx_trbs[streamid];
+		ringaddr = sctx_tr->ringaddr;
+		ccs = sctx_tr->ccs;
+		trb = XHCI_GADDR(sc, sctx_tr->ringaddr & ~0xFUL);
+		DPRINTF(("doorbell, stream %u, ccs %lx, trb ccs %x\r\n",
+		        streamid, ep_ctx->qwEpCtx2 & XHCI_TRB_3_CYCLE_BIT,
+		        trb->dwTrb3 & XHCI_TRB_3_CYCLE_BIT));
+	} else {
+		ringaddr = devep->ep_ringaddr;
+		ccs = devep->ep_ccs;
+		trb = devep->ep_tr;
+		DPRINTF(("doorbell, ccs %lx, trb ccs %x\r\n",
+		        ep_ctx->qwEpCtx2 & XHCI_TRB_3_CYCLE_BIT,
+		        trb->dwTrb3 & XHCI_TRB_3_CYCLE_BIT));
+	}
+
+	if (XHCI_TRB_3_TYPE_GET(trb->dwTrb3) == 0) {
+		DPRINTF(("pci_xhci: ring %lx trb[%lx] EP %u is RESERVED?\r\n",
+		        ep_ctx->qwEpCtx2, devep->ep_ringaddr, epid));
+		return;
+	}
+
+	pci_xhci_handle_transfer(sc, dev, devep, ep_ctx, trb, slot, epid,
+	                         ringaddr, ccs, streamid);
+}
+
+static void
+pci_xhci_dbregs_write(struct pci_xhci_softc *sc, uint64_t offset,
+    uint64_t value)
+{
+
+	offset = (offset - sc->dboff) / sizeof(uint32_t);
+
+	DPRINTF(("pci_xhci: doorbell write offset 0x%lx: 0x%lx\r\n",
+	        offset, value));
+
+	if (XHCI_HALTED(sc)) {
+		DPRINTF(("pci_xhci: controller halted\r\n"));
+		return;
+	}
+
+	if (offset == 0)
+		pci_xhci_complete_commands(sc);
+	else if (sc->portregs != NULL)
+		pci_xhci_device_doorbell(sc, offset,
+		   XHCI_DB_TARGET_GET(value), XHCI_DB_SID_GET(value));
+}
+
+static void
+pci_xhci_rtsregs_write(struct pci_xhci_softc *sc, uint64_t offset,
+    uint64_t value)
+{
+	struct pci_xhci_rtsregs *rts;
+
+	offset -= sc->rtsoff;
+
+	if (offset == 0) {
+		DPRINTF(("pci_xhci attempted write to MFINDEX\r\n"));
+		return;
+	}
+
+	DPRINTF(("pci_xhci: runtime regs write offset 0x%lx: 0x%lx\r\n",
+	        offset, value));
+
+	offset -= 0x20;		/* start of intrreg */
+
+	rts = &sc->rtsregs;
+
+	switch (offset) {
+	case 0x00:
+		if (value & XHCI_IMAN_INTR_PEND)
+			rts->intrreg.iman &= ~XHCI_IMAN_INTR_PEND;
+		rts->intrreg.iman = (value & XHCI_IMAN_INTR_ENA) |
+		                    (rts->intrreg.iman & XHCI_IMAN_INTR_PEND);
+
+		if (!(value & XHCI_IMAN_INTR_ENA))
+			pci_xhci_deassert_interrupt(sc);
+
+		break;
+
+	case 0x04:
+		rts->intrreg.imod = value;
+		break;
+
+	case 0x08:
+		rts->intrreg.erstsz = value & 0xFFFF;
+		break;
+
+	case 0x10:
+		/* ERSTBA low bits */
+		rts->intrreg.erstba = MASK_64_HI(sc->rtsregs.intrreg.erstba) |
+		                      (value & ~0x3F);
+		break;
+
+	case 0x14:
+		/* ERSTBA high bits */
+		rts->intrreg.erstba = (value << 32) |
+		    MASK_64_LO(sc->rtsregs.intrreg.erstba);
+
+		rts->erstba_p = XHCI_GADDR(sc,
+		                        sc->rtsregs.intrreg.erstba & ~0x3FUL);
+
+		rts->erst_p = XHCI_GADDR(sc,
+		              sc->rtsregs.erstba_p->qwEvrsTablePtr & ~0x3FUL);
+
+		rts->er_enq_idx = 0;
+		rts->er_events_cnt = 0;
+
+		DPRINTF(("pci_xhci: wr erstba erst (%p) ptr 0x%lx, sz %u\r\n",
+		        rts->erstba_p,
+		        rts->erstba_p->qwEvrsTablePtr,
+		        rts->erstba_p->dwEvrsTableSize));
+		break;
+
+	case 0x18:
+		/* ERDP low bits */
+		rts->intrreg.erdp =
+		    MASK_64_HI(sc->rtsregs.intrreg.erdp) |
+		    (rts->intrreg.erdp & XHCI_ERDP_LO_BUSY) |
+		    (value & ~0xF);
+		if (value & XHCI_ERDP_LO_BUSY) {
+			rts->intrreg.erdp &= ~XHCI_ERDP_LO_BUSY;
+			rts->intrreg.iman &= ~XHCI_IMAN_INTR_PEND;
+		}
+
+		rts->er_deq_seg = XHCI_ERDP_LO_SINDEX(value);
+
+		break;
+
+	case 0x1C:
+		/* ERDP high bits */
+		rts->intrreg.erdp = (value << 32) |
+		    MASK_64_LO(sc->rtsregs.intrreg.erdp);
+
+		if (rts->er_events_cnt > 0) {
+			uint64_t erdp;
+			uint32_t erdp_i;
+
+			erdp = rts->intrreg.erdp & ~0xF;
+			erdp_i = (erdp - rts->erstba_p->qwEvrsTablePtr) /
+			           sizeof(struct xhci_trb);
+
+			if (erdp_i <= rts->er_enq_idx)
+				rts->er_events_cnt = rts->er_enq_idx - erdp_i;
+			else
+				rts->er_events_cnt =
+				          rts->erstba_p->dwEvrsTableSize -
+				          (erdp_i - rts->er_enq_idx);
+
+			DPRINTF(("pci_xhci: erdp 0x%lx, events cnt %u\r\n",
+			        erdp, rts->er_events_cnt));
+		}
+
+		break;
+
+	default:
+		DPRINTF(("pci_xhci attempted write to RTS offset 0x%lx\r\n",
+		        offset));
+		break;
+	}
+}
+
+static uint64_t
+pci_xhci_portregs_read(struct pci_xhci_softc *sc, uint64_t offset)
+{
+	int port;
+	uint32_t *p;
+
+	if (sc->portregs == NULL)
+		return (0);
+
+	port = (offset - 0x3F0) / 0x10;
+
+	if (port > XHCI_MAX_DEVS) {
+		DPRINTF(("pci_xhci: portregs_read port %d >= XHCI_MAX_DEVS\r\n",
+		    port));
+
+		/* return default value for unused port */
+		return (XHCI_PS_SPEED_SET(3));
+	}
+
+	offset = (offset - 0x3F0) % 0x10;
+
+	p = &sc->portregs[port].portsc;
+	p += offset / sizeof(uint32_t);
+
+	DPRINTF(("pci_xhci: portregs read offset 0x%lx port %u -> 0x%x\r\n",
+	        offset, port, *p));
+
+	return (*p);
+}
+
+static void
+pci_xhci_hostop_write(struct pci_xhci_softc *sc, uint64_t offset,
+    uint64_t value)
+{
+	offset -= XHCI_CAPLEN;
+
+	if (offset < 0x400)
+		DPRINTF(("pci_xhci: hostop write offset 0x%lx: 0x%lx\r\n",
+		         offset, value));
+
+	switch (offset) {
+	case XHCI_USBCMD:
+		sc->opregs.usbcmd = pci_xhci_usbcmd_write(sc, value & 0x3F0F);
+		break;
+
+	case XHCI_USBSTS:
+		/* clear bits on write */
+		sc->opregs.usbsts &= ~(value &
+		      (XHCI_STS_HSE|XHCI_STS_EINT|XHCI_STS_PCD|XHCI_STS_SSS|
+		       XHCI_STS_RSS|XHCI_STS_SRE|XHCI_STS_CNR));
+		break;
+
+	case XHCI_PAGESIZE:
+		/* read only */
+		break;
+
+	case XHCI_DNCTRL:
+		sc->opregs.dnctrl = value & 0xFFFF;
+		break;
+
+	case XHCI_CRCR_LO:
+		if (sc->opregs.crcr & XHCI_CRCR_LO_CRR) {
+			sc->opregs.crcr &= ~(XHCI_CRCR_LO_CS|XHCI_CRCR_LO_CA);
+			sc->opregs.crcr |= value &
+			                   (XHCI_CRCR_LO_CS|XHCI_CRCR_LO_CA);
+		} else {
+			sc->opregs.crcr = MASK_64_HI(sc->opregs.crcr) |
+			           (value & (0xFFFFFFC0 | XHCI_CRCR_LO_RCS));
+		}
+		break;
+
+	case XHCI_CRCR_HI:
+		if (!(sc->opregs.crcr & XHCI_CRCR_LO_CRR)) {
+			sc->opregs.crcr = MASK_64_LO(sc->opregs.crcr) |
+			                  (value << 32);
+
+			sc->opregs.cr_p = XHCI_GADDR(sc,
+			                  sc->opregs.crcr & ~0xF);
+		}
+
+		if (sc->opregs.crcr & XHCI_CRCR_LO_CS) {
+			/* Stop operation of Command Ring */
+		}
+
+		if (sc->opregs.crcr & XHCI_CRCR_LO_CA) {
+			/* Abort command */
+		}
+
+		break;
+
+	case XHCI_DCBAAP_LO:
+		sc->opregs.dcbaap = MASK_64_HI(sc->opregs.dcbaap) |
+		                    (value & 0xFFFFFFC0);
+		break;
+
+	case XHCI_DCBAAP_HI:
+		sc->opregs.dcbaap =  MASK_64_LO(sc->opregs.dcbaap) |
+		                     (value << 32);
+		sc->opregs.dcbaa_p = XHCI_GADDR(sc, sc->opregs.dcbaap & ~0x3FUL);
+
+		DPRINTF(("pci_xhci: opregs dcbaap = 0x%lx (vaddr 0x%lx)\r\n",
+		    sc->opregs.dcbaap, (uint64_t)sc->opregs.dcbaa_p));
+		break;
+
+	case XHCI_CONFIG:
+		sc->opregs.config = value & 0x03FF;
+		break;
+
+	default:
+		if (offset >= 0x400)
+			pci_xhci_portregs_write(sc, offset, value);
+
+		break;
+	}
+}
+
+
+static void
+pci_xhci_write(struct vmctx *ctx, int vcpu, struct pci_devinst *pi,
+                int baridx, uint64_t offset, int size, uint64_t value)
+{
+	struct pci_xhci_softc *sc;
+
+	sc = pi->pi_arg;
+
+        assert(baridx == 0);
+
+
+        pthread_mutex_lock(&sc->mtx);
+	if (offset < XHCI_CAPLEN)	/* read only registers */
+                WPRINTF(("pci_xhci: write RO-CAPs offset %ld\r\n", offset));
+	else if (offset < sc->dboff)
+		pci_xhci_hostop_write(sc, offset, value);
+	else if (offset < sc->rtsoff)
+		pci_xhci_dbregs_write(sc, offset, value);
+	else if (offset < sc->regsend)
+		pci_xhci_rtsregs_write(sc, offset, value);
+	else
+                WPRINTF(("pci_xhci: write invalid offset %ld\r\n", offset));
+
+        pthread_mutex_unlock(&sc->mtx);
+}
+
+static uint64_t
+pci_xhci_hostcap_read(struct pci_xhci_softc *sc, uint64_t offset)
+{
+	uint64_t	value;
+
+	switch (offset) {
+	case XHCI_CAPLENGTH:	/* 0x00 */
+		value = sc->caplength;
+		break;
+
+	case XHCI_HCSPARAMS1:	/* 0x04 */
+		value = sc->hcsparams1;
+		break;
+
+	case XHCI_HCSPARAMS2:	/* 0x08 */
+		value = sc->hcsparams2;
+		break;
+
+	case XHCI_HCSPARAMS3:	/* 0x0C */
+		value = sc->hcsparams3;
+		break;
+
+	case XHCI_HCSPARAMS0:	/* 0x10 */
+		value = sc->hccparams1;
+		break;
+
+	case XHCI_DBOFF:	/* 0x14 */
+		value = sc->dboff;
+		break;
+
+	case XHCI_RTSOFF:	/* 0x18 */
+		value = sc->rtsoff;
+		break;
+
+	case XHCI_HCCPRAMS2:	/* 0x1C */
+		value = sc->hccparams2;
+		break;
+
+	default:
+		value = 0;
+		break;
+	}
+
+	DPRINTF(("pci_xhci: hostcap read offset 0x%lx -> 0x%lx\r\n",
+	        offset, value));
+
+	return (value);
+}
+
+static uint64_t
+pci_xhci_hostop_read(struct pci_xhci_softc *sc, uint64_t offset)
+{
+	uint64_t value;
+
+	offset = (offset - XHCI_CAPLEN);
+
+	switch (offset) {
+	case XHCI_USBCMD:	/* 0x00 */
+		value = sc->opregs.usbcmd;
+		break;
+
+	case XHCI_USBSTS:	/* 0x04 */
+		value = sc->opregs.usbsts;
+		break;
+
+	case XHCI_PAGESIZE:	/* 0x08 */
+		value = sc->opregs.pgsz;
+		break;
+
+	case XHCI_DNCTRL:	/* 0x14 */
+		value = sc->opregs.dnctrl;
+		break;
+
+	case XHCI_CRCR_LO:	/* 0x18 */
+		value = sc->opregs.crcr & XHCI_CRCR_LO_CRR;
+		break;
+
+	case XHCI_CRCR_HI:	/* 0x1C */
+		value = 0;
+		break;
+
+	case XHCI_DCBAAP_LO:	/* 0x30 */
+		value = sc->opregs.dcbaap & 0xFFFFFFFF;
+		break;
+
+	case XHCI_DCBAAP_HI:	/* 0x34 */
+		value = (sc->opregs.dcbaap >> 32) & 0xFFFFFFFF;
+		break;
+
+	case XHCI_CONFIG:	/* 0x38 */
+		value = sc->opregs.config;
+		break;
+
+	default:
+		if (offset >= 0x400)
+			value = pci_xhci_portregs_read(sc, offset);
+		else
+			value = 0;
+
+		break;
+	}
+
+	if (offset < 0x400)
+		DPRINTF(("pci_xhci: hostop read offset 0x%lx -> 0x%lx\r\n",
+		        offset, value));
+
+	return (value);
+}
+
+static uint64_t
+pci_xhci_dbregs_read(struct pci_xhci_softc *sc, uint64_t offset)
+{
+
+	/* read doorbell always returns 0 */
+	return (0);
+}
+
+static uint64_t
+pci_xhci_rtsregs_read(struct pci_xhci_softc *sc, uint64_t offset)
+{
+	uint32_t	value;
+
+	offset -= sc->rtsoff;
+	value = 0;
+
+	if (offset == XHCI_MFINDEX) {
+		value = sc->rtsregs.mfindex;
+	} else if (offset >= 0x20) {
+		int item;
+		uint32_t *p;
+
+		offset -= 0x20;
+		item = offset % 32;
+
+		assert(offset < sizeof(sc->rtsregs.intrreg));
+
+		p = &sc->rtsregs.intrreg.iman;
+		p += item / sizeof(uint32_t);
+		value = *p;
+	}
+
+	DPRINTF(("pci_xhci: rtsregs read offset 0x%lx -> 0x%x\r\n",
+	        offset, value));
+
+	return (value);
+}
+
+static uint64_t
+pci_xhci_xecp_read(struct pci_xhci_softc *sc, uint64_t offset)
+{
+	uint32_t	value;
+
+	offset -= sc->regsend;
+	value = 0;
+
+	switch (offset) {
+	case 0:
+		/* rev major | rev minor | next-cap | cap-id */
+		value = (0x02 << 24) | (4 << 8) | XHCI_ID_PROTOCOLS;
+		break;
+	case 4:
+		/* name string = "USB" */
+		value = 0x20425355;
+		break;
+	case 8:
+		/* psic | proto-defined | compat # | compat offset */
+		value = ((XHCI_MAX_DEVS/2) << 8) | sc->usb2_port_start;
+		break;
+	case 12:
+		break;
+	case 16:
+		/* rev major | rev minor | next-cap | cap-id */
+		value = (0x03 << 24) | XHCI_ID_PROTOCOLS;
+		break;
+	case 20:
+		/* name string = "USB" */
+		value = 0x20425355;
+		break;
+	case 24:
+		/* psic | proto-defined | compat # | compat offset */
+		value = ((XHCI_MAX_DEVS/2) << 8) | sc->usb3_port_start;
+		break;
+	case 28:
+		break;
+	default:
+		DPRINTF(("pci_xhci: xecp invalid offset 0x%lx\r\n", offset));
+		break;
+	}
+
+	DPRINTF(("pci_xhci: xecp read offset 0x%lx -> 0x%x\r\n",
+	        offset, value));
+
+	return (value);
+}
+
+
+static uint64_t
+pci_xhci_read(struct vmctx *ctx, int vcpu, struct pci_devinst *pi, int baridx,
+    uint64_t offset, int size)
+{
+	struct pci_xhci_softc *sc;
+	uint32_t	value;
+
+	sc = pi->pi_arg;
+
+        assert(baridx == 0);
+
+        pthread_mutex_lock(&sc->mtx);
+	if (offset < XHCI_CAPLEN)
+		value = pci_xhci_hostcap_read(sc, offset);
+	else if (offset < sc->dboff)
+		value = pci_xhci_hostop_read(sc, offset);
+	else if (offset < sc->rtsoff)
+		value = pci_xhci_dbregs_read(sc, offset);
+	else if (offset < sc->regsend)
+		value = pci_xhci_rtsregs_read(sc, offset);
+	else if (offset < (sc->regsend + 4*32))
+		value = pci_xhci_xecp_read(sc, offset);
+	else {
+		value = 0;
+                WPRINTF(("pci_xhci: read invalid offset %ld\r\n", offset));
+	}
+
+        pthread_mutex_unlock(&sc->mtx);
+
+	switch (size) {
+	case 1:
+		value &= 0xFF;
+		break;
+	case 2:
+		value &= 0xFFFF;
+		break;
+	case 4:
+		value &= 0xFFFFFFFF;
+		break;
+	}
+
+	return (value);
+}
+
+static void
+pci_xhci_reset_port(struct pci_xhci_softc *sc, int portn, int warm)
+{
+	struct pci_xhci_portregs *port;
+	struct pci_xhci_dev_emu	*dev;
+	struct xhci_trb		evtrb;
+	int	error;
+
+	assert(portn <= XHCI_MAX_DEVS);
+
+	DPRINTF(("xhci reset port %d\r\n", portn));
+
+	port = XHCI_PORTREG_PTR(sc, portn);
+	dev = XHCI_DEVINST_PTR(sc, portn);
+	if (dev) {
+		port->portsc &= ~(XHCI_PS_PLS_MASK | XHCI_PS_PR | XHCI_PS_PRC);
+		port->portsc |= XHCI_PS_PED |
+		    XHCI_PS_SPEED_SET(dev->dev_ue->ue_usbspeed);
+
+		if (warm && dev->dev_ue->ue_usbver == 3) {
+			port->portsc |= XHCI_PS_WRC;
+		}
+
+		if ((port->portsc & XHCI_PS_PRC) == 0) {
+			port->portsc |= XHCI_PS_PRC;
+
+			pci_xhci_set_evtrb(&evtrb, portn,
+			     XHCI_TRB_ERROR_SUCCESS,
+			     XHCI_TRB_EVENT_PORT_STS_CHANGE);
+			error = pci_xhci_insert_event(sc, &evtrb, 1);
+			if (error != XHCI_TRB_ERROR_SUCCESS)
+				DPRINTF(("xhci reset port insert event "
+				         "failed\r\n"));
+		}
+	}
+}
+
+static void
+pci_xhci_init_port(struct pci_xhci_softc *sc, int portn)
+{
+	struct pci_xhci_portregs *port;
+	struct pci_xhci_dev_emu	*dev;
+
+	port = XHCI_PORTREG_PTR(sc, portn);
+	dev = XHCI_DEVINST_PTR(sc, portn);
+	if (dev) {
+		port->portsc = XHCI_PS_CCS |		/* connected */
+		               XHCI_PS_PP;		/* port power */
+		
+		if (dev->dev_ue->ue_usbver == 2) {
+			port->portsc |= XHCI_PS_PLS_SET(UPS_PORT_LS_POLL) |
+		               XHCI_PS_SPEED_SET(dev->dev_ue->ue_usbspeed);
+		} else {
+			port->portsc |= XHCI_PS_PLS_SET(UPS_PORT_LS_U0) |
+		               XHCI_PS_PED |		/* enabled */
+		               XHCI_PS_SPEED_SET(dev->dev_ue->ue_usbspeed);
+		}
+		
+		DPRINTF(("Init port %d 0x%x\n", portn, port->portsc));
+	} else {
+		port->portsc = XHCI_PS_PLS_SET(UPS_PORT_LS_RX_DET) | XHCI_PS_PP;
+		DPRINTF(("Init empty port %d 0x%x\n", portn, port->portsc));
+	}
+}
+
+static int
+pci_xhci_dev_intr(struct usb_hci *hci, int epctx)
+{
+	struct pci_xhci_dev_emu *dev;
+	struct xhci_trb		evtrb;
+	struct pci_xhci_softc	*sc;
+	struct pci_xhci_portregs *p;
+	int	error;
+	int	dir_in;
+	int	epid;
+
+	dir_in = epctx & 0x80;
+	epid = epctx & ~0x80;
+
+	/* HW endpoint contexts are 0-15; convert to epid based on dir */
+	epid = (epid * 2) + (dir_in ? 1 : 0);
+
+	assert(epid >= 1 && epid <= 31);
+
+	dev = hci->hci_sc;
+	sc = dev->xsc;
+
+	/* check if device is ready; OS has to initialise it */
+	if (sc->rtsregs.erstba_p == NULL ||
+	    (sc->opregs.usbcmd & XHCI_CMD_RS) == 0)
+		return (0);
+
+	p = XHCI_PORTREG_PTR(sc, hci->hci_port);
+
+	/* raise event if link U3 (suspended) state */
+	if (XHCI_PS_PLS_GET(p->portsc) == 3) {
+		p->portsc &= ~XHCI_PS_PLS_MASK;
+		p->portsc |= XHCI_PS_PLS_SET(UPS_PORT_LS_RESUME);
+		if ((p->portsc & XHCI_PS_PLC) != 0)
+			return (0);
+
+		p->portsc |= XHCI_PS_PLC;
+
+		pci_xhci_set_evtrb(&evtrb, hci->hci_port,
+		      XHCI_TRB_ERROR_SUCCESS, XHCI_TRB_EVENT_PORT_STS_CHANGE);
+		error = pci_xhci_insert_event(sc, &evtrb, 0);
+		if (error != XHCI_TRB_ERROR_SUCCESS)
+			goto done;
+	}
+
+	DPRINTF(("xhci device interrupt on endpoint %d\r\n", epid));
+
+	pci_xhci_device_doorbell(sc, hci->hci_port, epid, 0);
+
+done:
+	return (error);
+}
+
+static int
+pci_xhci_dev_event(struct usb_hci *hci, enum hci_usbev evid, void *param)
+{
+
+	DPRINTF(("xhci device event port %d\r\n", hci->hci_port));
+	return (0);
+}
+
+
+
+static void
+pci_xhci_device_usage(char *opt)
+{
+
+	fprintf(stderr, "Invalid USB emulation \"%s\"\r\n", opt);
+}
+
+static int
+pci_xhci_parse_opts(struct pci_xhci_softc *sc, char *opts)
+{
+	struct pci_xhci_dev_emu	**devices;
+	struct pci_xhci_dev_emu	*dev;
+	struct usb_devemu	*ue;
+	void	*devsc;
+	char	*uopt, *xopts, *config;
+	int	usb3_port, usb2_port, i;
+
+	usb3_port = sc->usb3_port_start - 1;
+	usb2_port = sc->usb2_port_start - 1;
+	devices = NULL;
+
+	if (opts == NULL)
+		goto portsfinal;
+
+	devices = calloc(XHCI_MAX_DEVS, sizeof(struct pci_xhci_dev_emu *));
+
+	sc->slots = calloc(XHCI_MAX_SLOTS, sizeof(struct pci_xhci_dev_emu *));
+	sc->devices = devices;
+	sc->ndevices = 0;
+
+	uopt = strdup(opts);
+	for (xopts = strtok(uopt, ",");
+	     xopts != NULL;
+	     xopts = strtok(NULL, ",")) {
+		if (usb2_port == ((sc->usb2_port_start-1) + XHCI_MAX_DEVS/2) ||
+		    usb3_port == ((sc->usb3_port_start-1) + XHCI_MAX_DEVS/2)) {
+			WPRINTF(("pci_xhci max number of USB 2 or 3 "
+			     "devices reached, max %d\r\n", XHCI_MAX_DEVS/2));
+			usb2_port = usb3_port = -1;
+			goto done;
+		}
+
+		/* device[=<config>] */
+		if ((config = strchr(xopts, '=')) == NULL)
+			config = "";		/* no config */
+		else
+			*config++ = '\0';
+
+		ue = usb_emu_finddev(xopts);
+		if (ue == NULL) {
+			pci_xhci_device_usage(xopts);
+			DPRINTF(("pci_xhci device not found %s\r\n", xopts));
+			usb2_port = usb3_port = -1;
+			goto done;
+		}
+
+		DPRINTF(("pci_xhci adding device %s, opts \"%s\"\r\n",
+		        xopts, config));
+
+		dev = calloc(1, sizeof(struct pci_xhci_dev_emu));
+		dev->xsc = sc;
+		dev->hci.hci_sc = dev;
+		dev->hci.hci_intr = pci_xhci_dev_intr;
+		dev->hci.hci_event = pci_xhci_dev_event;
+
+		if (ue->ue_usbver == 2) {
+			dev->hci.hci_port = usb2_port + 1;
+			devices[usb2_port] = dev;
+			usb2_port++;
+		} else {
+			dev->hci.hci_port = usb3_port + 1;
+			devices[usb3_port] = dev;
+			usb3_port++;
+		}
+
+		dev->hci.hci_address = 0;
+		devsc = ue->ue_init(&dev->hci, config);
+		if (devsc == NULL) {
+			pci_xhci_device_usage(xopts);
+			usb2_port = usb3_port = -1;
+			goto done;
+		}
+
+		dev->dev_ue = ue;
+		dev->dev_sc = devsc;
+
+		/* assign slot number to device */
+		sc->slots[sc->ndevices] = dev;
+
+		sc->ndevices++;
+	}
+
+portsfinal:
+	sc->portregs = calloc(XHCI_MAX_DEVS, sizeof(struct pci_xhci_portregs));
+
+	if (sc->ndevices > 0) {
+		/* port and slot numbering start from 1 */
+		sc->devices--;
+		sc->portregs--;
+		sc->slots--;
+
+		for (i = 1; i <= XHCI_MAX_DEVS; i++) {
+			pci_xhci_init_port(sc, i);
+		}
+	} else {
+		WPRINTF(("pci_xhci no USB devices configured\r\n"));
+		sc->ndevices = 1;
+	}
+
+done:
+	if (devices != NULL) {
+		if (usb2_port <= 0 && usb3_port <= 0) {
+			sc->devices = NULL;
+			for (i = 0; devices[i] != NULL; i++)
+				free(devices[i]);
+			sc->ndevices = -1;
+
+			free(devices);
+		}
+	}
+	return (sc->ndevices);
+}
+
+static int
+pci_xhci_init(struct vmctx *ctx, struct pci_devinst *pi, char *opts)
+{
+	struct pci_xhci_softc *sc;
+	int	error;
+
+	if (xhci_in_use) {
+		WPRINTF(("pci_xhci controller already defined\r\n"));
+		return (-1);
+	}
+	xhci_in_use = 1;
+
+	sc = calloc(1, sizeof(struct pci_xhci_softc));
+	pi->pi_arg = sc;
+	sc->xsc_pi = pi;
+
+	sc->usb2_port_start = (XHCI_MAX_DEVS/2) + 1;
+	sc->usb3_port_start = 1;
+
+	/* discover devices */
+	error = pci_xhci_parse_opts(sc, opts);
+	if (error < 0)
+		goto done;
+	else
+		error = 0;
+
+	sc->caplength = XHCI_SET_CAPLEN(XHCI_CAPLEN) |
+	                XHCI_SET_HCIVERSION(0x0100);
+	sc->hcsparams1 = XHCI_SET_HCSP1_MAXPORTS(XHCI_MAX_DEVS) |
+	                 XHCI_SET_HCSP1_MAXINTR(1) |	/* interrupters */
+	                 XHCI_SET_HCSP1_MAXSLOTS(XHCI_MAX_SLOTS);
+	sc->hcsparams2 = XHCI_SET_HCSP2_ERSTMAX(XHCI_ERST_MAX) |
+	                 XHCI_SET_HCSP2_IST(0x04);
+	sc->hcsparams3 = 0;				/* no latency */
+	sc->hccparams1 = XHCI_SET_HCCP1_NSS(1) |	/* no 2nd-streams */
+	                 XHCI_SET_HCCP1_SPC(1) |	/* short packet */
+	                 XHCI_SET_HCCP1_MAXPSA(XHCI_STREAMS_MAX);
+	sc->hccparams2 = XHCI_SET_HCCP2_LEC(1) |
+	                 XHCI_SET_HCCP2_U3C(1);
+	sc->dboff = XHCI_SET_DOORBELL(XHCI_CAPLEN + XHCI_PORTREGS_START +
+	            XHCI_MAX_DEVS * sizeof(struct pci_xhci_portregs));
+
+	/* dboff must be 32-bit aligned */
+	if (sc->dboff & 0x3)
+		sc->dboff = (sc->dboff + 0x3) & ~0x3;
+
+	/* rtsoff must be 32-bytes aligned */
+	sc->rtsoff = XHCI_SET_RTSOFFSET(sc->dboff + (XHCI_MAX_SLOTS+1) * 32);
+	if (sc->rtsoff & 0x1F)
+		sc->rtsoff = (sc->rtsoff + 0x1F) & ~0x1F;
+
+	DPRINTF(("pci_xhci dboff: 0x%x, rtsoff: 0x%x\r\n", sc->dboff,
+	        sc->rtsoff));
+
+	sc->opregs.usbsts = XHCI_STS_HCH;
+	sc->opregs.pgsz = XHCI_PAGESIZE_4K;
+
+	pci_xhci_reset(sc);
+
+	sc->regsend = sc->rtsoff + 0x20 + 32;		/* only 1 intrpter */
+
+	/*
+	 * Set extended capabilities pointer to be after regsend;
+	 * value of xecp field is 32-bit offset.
+	 */
+	sc->hccparams1 |= XHCI_SET_HCCP1_XECP(sc->regsend/4);
+
+	pci_set_cfgdata16(pi, PCIR_DEVICE, 0x1E31);
+	pci_set_cfgdata16(pi, PCIR_VENDOR, 0x8086);
+	pci_set_cfgdata8(pi, PCIR_CLASS, PCIC_SERIALBUS);
+	pci_set_cfgdata8(pi, PCIR_SUBCLASS, PCIS_SERIALBUS_USB);
+	pci_set_cfgdata8(pi, PCIR_PROGIF,PCIP_SERIALBUS_USB_XHCI);
+	pci_set_cfgdata8(pi, PCI_USBREV, PCI_USB_REV_3_0);
+
+	pci_emul_add_msicap(pi, 1);
+
+	/* regsend + xecp registers */
+	pci_emul_alloc_bar(pi, 0, PCIBAR_MEM32, sc->regsend + 4*32);
+	DPRINTF(("pci_xhci pci_emu_alloc: %d\r\n", sc->regsend + 4*32));
+
+
+	pci_lintr_request(pi);
+
+	pthread_mutex_init(&sc->mtx, NULL);
+
+done:
+	if (error) {
+		free(sc);
+	}
+
+	return (error);
+}
+
+
+
+struct pci_devemu pci_de_xhci = {
+	.pe_emu =	"xhci",
+	.pe_init =	pci_xhci_init,
+	.pe_barwrite =	pci_xhci_write,
+	.pe_barread =	pci_xhci_read
+};
+PCI_EMUL_SET(pci_de_xhci);
diff --git a/pci_xhci.h b/pci_xhci.h
new file mode 100644
index 000000000000..d5f05af5d029
--- /dev/null
+++ b/pci_xhci.h
@@ -0,0 +1,353 @@
+/*-
+ * Copyright (c) 2014 Leon Dang <ldang@nahannisys.com>
+ * 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$
+ */
+
+#ifndef	_PCI_XHCI_H_
+#define	_PCI_XHCI_H_
+
+#define	PCI_USBREV		0x60	/* USB protocol revision */
+
+
+enum {					/* dsc_slotstate */
+	XHCI_ST_DISABLED,
+	XHCI_ST_ENABLED,
+	XHCI_ST_DEFAULT,
+	XHCI_ST_ADDRESSED,
+	XHCI_ST_CONFIGURED,
+	XHCI_ST_MAX
+};
+
+enum {
+	XHCI_ST_SLCTX_DISABLED,
+	XHCI_ST_SLCTX_DEFAULT,
+	XHCI_ST_SLCTX_ADDRESSED,
+	XHCI_ST_SLCTX_CONFIGURED
+};
+
+enum {
+	XHCI_ST_EPCTX_DISABLED,
+	XHCI_ST_EPCTX_RUNNING,
+	XHCI_ST_EPCTX_HALTED,
+	XHCI_ST_EPCTX_STOPPED,
+	XHCI_ST_EPCTX_ERROR
+};
+
+#define	XHCI_MAX_DEVICES	MIN(USB_MAX_DEVICES, 128)
+#define	XHCI_MAX_ENDPOINTS	32	/* hardcoded - do not change */
+#define	XHCI_MAX_SCRATCHPADS	32
+#define	XHCI_MAX_EVENTS		(16 * 13)
+#define	XHCI_MAX_COMMANDS	(16 * 1)
+#define	XHCI_MAX_RSEG		1
+#define	XHCI_MAX_TRANSFERS	4
+#if USB_MAX_EP_STREAMS == 8
+#define	XHCI_MAX_STREAMS	8
+#define	XHCI_MAX_STREAMS_LOG	3
+#elif USB_MAX_EP_STREAMS == 1
+#define	XHCI_MAX_STREAMS	1
+#define	XHCI_MAX_STREAMS_LOG	0
+#else
+#error "The USB_MAX_EP_STREAMS value is not supported."
+#endif
+#define	XHCI_DEV_CTX_ADDR_ALIGN		64	/* bytes */
+#define	XHCI_DEV_CTX_ALIGN		64	/* bytes */
+#define	XHCI_INPUT_CTX_ALIGN		64	/* bytes */
+#define	XHCI_SLOT_CTX_ALIGN		32	/* bytes */
+#define	XHCI_ENDP_CTX_ALIGN		32	/* bytes */
+#define	XHCI_STREAM_CTX_ALIGN		16	/* bytes */
+#define	XHCI_TRANS_RING_SEG_ALIGN	16	/* bytes */
+#define	XHCI_CMD_RING_SEG_ALIGN		64	/* bytes */
+#define	XHCI_EVENT_RING_SEG_ALIGN	64	/* bytes */
+#define	XHCI_SCRATCH_BUF_ARRAY_ALIGN	64	/* bytes */
+#define	XHCI_SCRATCH_BUFFER_ALIGN	USB_PAGE_SIZE
+#define	XHCI_TRB_ALIGN			16	/* bytes */
+#define	XHCI_TD_ALIGN			64	/* bytes */
+#define	XHCI_PAGE_SIZE			4096	/* bytes */
+
+struct xhci_slot_ctx {
+	volatile uint32_t	dwSctx0;
+#define	XHCI_SCTX_0_ROUTE_SET(x)		((x) & 0xFFFFF)
+#define	XHCI_SCTX_0_ROUTE_GET(x)		((x) & 0xFFFFF)
+#define	XHCI_SCTX_0_SPEED_SET(x)		(((x) & 0xF) << 20)
+#define	XHCI_SCTX_0_SPEED_GET(x)		(((x) >> 20) & 0xF)
+#define	XHCI_SCTX_0_MTT_SET(x)			(((x) & 0x1) << 25)
+#define	XHCI_SCTX_0_MTT_GET(x)			(((x) >> 25) & 0x1)
+#define	XHCI_SCTX_0_HUB_SET(x)			(((x) & 0x1) << 26)
+#define	XHCI_SCTX_0_HUB_GET(x)			(((x) >> 26) & 0x1)
+#define	XHCI_SCTX_0_CTX_NUM_SET(x)		(((x) & 0x1F) << 27)
+#define	XHCI_SCTX_0_CTX_NUM_GET(x)		(((x) >> 27) & 0x1F)
+	volatile uint32_t	dwSctx1;
+#define	XHCI_SCTX_1_MAX_EL_SET(x)		((x) & 0xFFFF)
+#define	XHCI_SCTX_1_MAX_EL_GET(x)		((x) & 0xFFFF)
+#define	XHCI_SCTX_1_RH_PORT_SET(x)		(((x) & 0xFF) << 16)
+#define	XHCI_SCTX_1_RH_PORT_GET(x)		(((x) >> 16) & 0xFF)
+#define	XHCI_SCTX_1_NUM_PORTS_SET(x)		(((x) & 0xFF) << 24)
+#define	XHCI_SCTX_1_NUM_PORTS_GET(x)		(((x) >> 24) & 0xFF)
+	volatile uint32_t	dwSctx2;
+#define	XHCI_SCTX_2_TT_HUB_SID_SET(x)		((x) & 0xFF)
+#define	XHCI_SCTX_2_TT_HUB_SID_GET(x)		((x) & 0xFF)
+#define	XHCI_SCTX_2_TT_PORT_NUM_SET(x)		(((x) & 0xFF) << 8)
+#define	XHCI_SCTX_2_TT_PORT_NUM_GET(x)		(((x) >> 8) & 0xFF)
+#define	XHCI_SCTX_2_TT_THINK_TIME_SET(x)	(((x) & 0x3) << 16)
+#define	XHCI_SCTX_2_TT_THINK_TIME_GET(x)	(((x) >> 16) & 0x3)
+#define	XHCI_SCTX_2_IRQ_TARGET_SET(x)		(((x) & 0x3FF) << 22)
+#define	XHCI_SCTX_2_IRQ_TARGET_GET(x)		(((x) >> 22) & 0x3FF)
+	volatile uint32_t	dwSctx3;
+#define	XHCI_SCTX_3_DEV_ADDR_SET(x)		((x) & 0xFF)
+#define	XHCI_SCTX_3_DEV_ADDR_GET(x)		((x) & 0xFF)
+#define	XHCI_SCTX_3_SLOT_STATE_SET(x)		(((x) & 0x1F) << 27)
+#define	XHCI_SCTX_3_SLOT_STATE_GET(x)		(((x) >> 27) & 0x1F)
+	volatile uint32_t	dwSctx4;
+	volatile uint32_t	dwSctx5;
+	volatile uint32_t	dwSctx6;
+	volatile uint32_t	dwSctx7;
+};
+
+struct xhci_endp_ctx {
+	volatile uint32_t	dwEpCtx0;
+#define	XHCI_EPCTX_0_EPSTATE_SET(x)		((x) & 0x7)
+#define	XHCI_EPCTX_0_EPSTATE_GET(x)		((x) & 0x7)
+#define	XHCI_EPCTX_0_MULT_SET(x)		(((x) & 0x3) << 8)
+#define	XHCI_EPCTX_0_MULT_GET(x)		(((x) >> 8) & 0x3)
+#define	XHCI_EPCTX_0_MAXP_STREAMS_SET(x)	(((x) & 0x1F) << 10)
+#define	XHCI_EPCTX_0_MAXP_STREAMS_GET(x)	(((x) >> 10) & 0x1F)
+#define	XHCI_EPCTX_0_LSA_SET(x)			(((x) & 0x1) << 15)
+#define	XHCI_EPCTX_0_LSA_GET(x)			(((x) >> 15) & 0x1)
+#define	XHCI_EPCTX_0_IVAL_SET(x)		(((x) & 0xFF) << 16)
+#define	XHCI_EPCTX_0_IVAL_GET(x)		(((x) >> 16) & 0xFF)
+	volatile uint32_t	dwEpCtx1;
+#define	XHCI_EPCTX_1_CERR_SET(x)		(((x) & 0x3) << 1)
+#define	XHCI_EPCTX_1_CERR_GET(x)		(((x) >> 1) & 0x3)
+#define	XHCI_EPCTX_1_EPTYPE_SET(x)		(((x) & 0x7) << 3)
+#define	XHCI_EPCTX_1_EPTYPE_GET(x)		(((x) >> 3) & 0x7)
+#define	XHCI_EPCTX_1_HID_SET(x)			(((x) & 0x1) << 7)
+#define	XHCI_EPCTX_1_HID_GET(x)			(((x) >> 7) & 0x1)
+#define	XHCI_EPCTX_1_MAXB_SET(x)		(((x) & 0xFF) << 8)
+#define	XHCI_EPCTX_1_MAXB_GET(x)		(((x) >> 8) & 0xFF)
+#define	XHCI_EPCTX_1_MAXP_SIZE_SET(x)		(((x) & 0xFFFF) << 16)
+#define	XHCI_EPCTX_1_MAXP_SIZE_GET(x)		(((x) >> 16) & 0xFFFF)
+	volatile uint64_t	qwEpCtx2;
+#define	XHCI_EPCTX_2_DCS_SET(x)			((x) & 0x1)
+#define	XHCI_EPCTX_2_DCS_GET(x)			((x) & 0x1)
+#define	XHCI_EPCTX_2_TR_DQ_PTR_MASK		0xFFFFFFFFFFFFFFF0U
+	volatile uint32_t	dwEpCtx4;
+#define	XHCI_EPCTX_4_AVG_TRB_LEN_SET(x)		((x) & 0xFFFF)
+#define	XHCI_EPCTX_4_AVG_TRB_LEN_GET(x)		((x) & 0xFFFF)
+#define	XHCI_EPCTX_4_MAX_ESIT_PAYLOAD_SET(x)	(((x) & 0xFFFF) << 16)
+#define	XHCI_EPCTX_4_MAX_ESIT_PAYLOAD_GET(x)	(((x) >> 16) & 0xFFFF)
+	volatile uint32_t	dwEpCtx5;
+	volatile uint32_t	dwEpCtx6;
+	volatile uint32_t	dwEpCtx7;
+};
+
+struct xhci_input_ctx {
+#define	XHCI_INCTX_NON_CTRL_MASK	0xFFFFFFFCU
+	volatile uint32_t	dwInCtx0;
+#define	XHCI_INCTX_0_DROP_MASK(n)	(1U << (n))
+	volatile uint32_t	dwInCtx1;
+#define	XHCI_INCTX_1_ADD_MASK(n)	(1U << (n))
+	volatile uint32_t	dwInCtx2;
+	volatile uint32_t	dwInCtx3;
+	volatile uint32_t	dwInCtx4;
+	volatile uint32_t	dwInCtx5;
+	volatile uint32_t	dwInCtx6;
+	volatile uint32_t	dwInCtx7;
+};
+
+struct xhci_input_dev_ctx {
+	struct xhci_input_ctx	ctx_input;
+	union {
+		struct xhci_slot_ctx	u_slot;
+		struct xhci_endp_ctx	u_ep[XHCI_MAX_ENDPOINTS];
+	} ctx_dev_slep;
+};
+
+struct xhci_dev_ctx {
+	union {
+		struct xhci_slot_ctx	u_slot;
+		struct xhci_endp_ctx	u_ep[XHCI_MAX_ENDPOINTS];
+	} ctx_dev_slep;
+} __aligned(XHCI_DEV_CTX_ALIGN);
+#define	ctx_slot	ctx_dev_slep.u_slot
+#define	ctx_ep		ctx_dev_slep.u_ep
+
+struct xhci_stream_ctx {
+	volatile uint64_t	qwSctx0;
+#define	XHCI_SCTX_0_DCS_GET(x)		((x) & 0x1)
+#define	XHCI_SCTX_0_DCS_SET(x)		((x) & 0x1)
+#define	XHCI_SCTX_0_SCT_SET(x)		(((x) & 0x7) << 1)
+#define	XHCI_SCTX_0_SCT_GET(x)		(((x) >> 1) & 0x7)
+#define	XHCI_SCTX_0_SCT_SEC_TR_RING	0x0
+#define	XHCI_SCTX_0_SCT_PRIM_TR_RING	0x1
+#define	XHCI_SCTX_0_SCT_PRIM_SSA_8	0x2
+#define	XHCI_SCTX_0_SCT_PRIM_SSA_16	0x3
+#define	XHCI_SCTX_0_SCT_PRIM_SSA_32	0x4
+#define	XHCI_SCTX_0_SCT_PRIM_SSA_64	0x5
+#define	XHCI_SCTX_0_SCT_PRIM_SSA_128	0x6
+#define	XHCI_SCTX_0_SCT_PRIM_SSA_256	0x7
+#define	XHCI_SCTX_0_TR_DQ_PTR_MASK	0xFFFFFFFFFFFFFFF0U
+	volatile uint32_t	dwSctx2;
+	volatile uint32_t	dwSctx3;
+};
+
+struct xhci_trb {
+	volatile uint64_t	qwTrb0;
+#define	XHCI_TRB_0_DIR_IN_MASK		(0x80ULL << 0)
+#define	XHCI_TRB_0_WLENGTH_MASK		(0xFFFFULL << 48)
+	volatile uint32_t	dwTrb2;
+#define	XHCI_TRB_2_ERROR_GET(x)		(((x) >> 24) & 0xFF)
+#define	XHCI_TRB_2_ERROR_SET(x)		(((x) & 0xFF) << 24)
+#define	XHCI_TRB_2_TDSZ_GET(x)		(((x) >> 17) & 0x1F)
+#define	XHCI_TRB_2_TDSZ_SET(x)		(((x) & 0x1F) << 17)
+#define	XHCI_TRB_2_REM_GET(x)		((x) & 0xFFFFFF)
+#define	XHCI_TRB_2_REM_SET(x)		((x) & 0xFFFFFF)
+#define	XHCI_TRB_2_BYTES_GET(x)		((x) & 0x1FFFF)
+#define	XHCI_TRB_2_BYTES_SET(x)		((x) & 0x1FFFF)
+#define	XHCI_TRB_2_IRQ_GET(x)		(((x) >> 22) & 0x3FF)
+#define	XHCI_TRB_2_IRQ_SET(x)		(((x) & 0x3FF) << 22)
+#define	XHCI_TRB_2_STREAM_GET(x)	(((x) >> 16) & 0xFFFF)
+#define	XHCI_TRB_2_STREAM_SET(x)	(((x) & 0xFFFF) << 16)
+
+	volatile uint32_t	dwTrb3;
+#define	XHCI_TRB_3_TYPE_GET(x)		(((x) >> 10) & 0x3F)
+#define	XHCI_TRB_3_TYPE_SET(x)		(((x) & 0x3F) << 10)
+#define	XHCI_TRB_3_CYCLE_BIT		(1U << 0)
+#define	XHCI_TRB_3_TC_BIT		(1U << 1)	/* command ring only */
+#define	XHCI_TRB_3_ENT_BIT		(1U << 1)	/* transfer ring only */
+#define	XHCI_TRB_3_ISP_BIT		(1U << 2)
+#define	XHCI_TRB_3_ED_BIT		(1U << 2)
+#define	XHCI_TRB_3_NSNOOP_BIT		(1U << 3)
+#define	XHCI_TRB_3_CHAIN_BIT		(1U << 4)
+#define	XHCI_TRB_3_IOC_BIT		(1U << 5)
+#define	XHCI_TRB_3_IDT_BIT		(1U << 6)
+#define	XHCI_TRB_3_TBC_GET(x)		(((x) >> 7) & 3)
+#define	XHCI_TRB_3_TBC_SET(x)		(((x) & 3) << 7)
+#define	XHCI_TRB_3_BEI_BIT		(1U << 9)
+#define	XHCI_TRB_3_DCEP_BIT		(1U << 9)
+#define	XHCI_TRB_3_PRSV_BIT		(1U << 9)
+#define	XHCI_TRB_3_BSR_BIT		(1U << 9)
+#define	XHCI_TRB_3_TRT_MASK		(3U << 16)
+#define	XHCI_TRB_3_TRT_NONE		(0U << 16)
+#define	XHCI_TRB_3_TRT_OUT		(2U << 16)
+#define	XHCI_TRB_3_TRT_IN		(3U << 16)
+#define	XHCI_TRB_3_DIR_IN		(1U << 16)
+#define	XHCI_TRB_3_TLBPC_GET(x)		(((x) >> 16) & 0xF)
+#define	XHCI_TRB_3_TLBPC_SET(x)		(((x) & 0xF) << 16)
+#define	XHCI_TRB_3_EP_GET(x)		(((x) >> 16) & 0x1F)
+#define	XHCI_TRB_3_EP_SET(x)		(((x) & 0x1F) << 16)
+#define	XHCI_TRB_3_FRID_GET(x)		(((x) >> 20) & 0x7FF)
+#define	XHCI_TRB_3_FRID_SET(x)		(((x) & 0x7FF) << 20)
+#define	XHCI_TRB_3_ISO_SIA_BIT		(1U << 31)
+#define	XHCI_TRB_3_SUSP_EP_BIT		(1U << 23)
+#define	XHCI_TRB_3_SLOT_GET(x)		(((x) >> 24) & 0xFF)
+#define	XHCI_TRB_3_SLOT_SET(x)		(((x) & 0xFF) << 24)
+
+/* Commands */
+#define	XHCI_TRB_TYPE_RESERVED		0x00
+#define	XHCI_TRB_TYPE_NORMAL		0x01
+#define	XHCI_TRB_TYPE_SETUP_STAGE	0x02
+#define	XHCI_TRB_TYPE_DATA_STAGE	0x03
+#define	XHCI_TRB_TYPE_STATUS_STAGE	0x04
+#define	XHCI_TRB_TYPE_ISOCH		0x05
+#define	XHCI_TRB_TYPE_LINK		0x06
+#define	XHCI_TRB_TYPE_EVENT_DATA	0x07
+#define	XHCI_TRB_TYPE_NOOP		0x08
+#define	XHCI_TRB_TYPE_ENABLE_SLOT	0x09
+#define	XHCI_TRB_TYPE_DISABLE_SLOT	0x0A
+#define	XHCI_TRB_TYPE_ADDRESS_DEVICE	0x0B
+#define	XHCI_TRB_TYPE_CONFIGURE_EP	0x0C
+#define	XHCI_TRB_TYPE_EVALUATE_CTX	0x0D
+#define	XHCI_TRB_TYPE_RESET_EP		0x0E
+#define	XHCI_TRB_TYPE_STOP_EP		0x0F
+#define	XHCI_TRB_TYPE_SET_TR_DEQUEUE	0x10
+#define	XHCI_TRB_TYPE_RESET_DEVICE	0x11
+#define	XHCI_TRB_TYPE_FORCE_EVENT	0x12
+#define	XHCI_TRB_TYPE_NEGOTIATE_BW	0x13
+#define	XHCI_TRB_TYPE_SET_LATENCY_TOL  	0x14
+#define	XHCI_TRB_TYPE_GET_PORT_BW	0x15
+#define	XHCI_TRB_TYPE_FORCE_HEADER	0x16
+#define	XHCI_TRB_TYPE_NOOP_CMD		0x17
+
+/* Events */
+#define	XHCI_TRB_EVENT_TRANSFER		0x20
+#define	XHCI_TRB_EVENT_CMD_COMPLETE	0x21
+#define	XHCI_TRB_EVENT_PORT_STS_CHANGE  0x22
+#define	XHCI_TRB_EVENT_BW_REQUEST      	0x23
+#define	XHCI_TRB_EVENT_DOORBELL		0x24
+#define	XHCI_TRB_EVENT_HOST_CTRL	0x25
+#define	XHCI_TRB_EVENT_DEVICE_NOTIFY	0x26
+#define	XHCI_TRB_EVENT_MFINDEX_WRAP	0x27
+
+/* Error codes */
+#define	XHCI_TRB_ERROR_INVALID		0x00
+#define	XHCI_TRB_ERROR_SUCCESS		0x01
+#define	XHCI_TRB_ERROR_DATA_BUF		0x02
+#define	XHCI_TRB_ERROR_BABBLE		0x03
+#define	XHCI_TRB_ERROR_XACT		0x04
+#define	XHCI_TRB_ERROR_TRB		0x05
+#define	XHCI_TRB_ERROR_STALL		0x06
+#define	XHCI_TRB_ERROR_RESOURCE		0x07
+#define	XHCI_TRB_ERROR_BANDWIDTH	0x08
+#define	XHCI_TRB_ERROR_NO_SLOTS		0x09
+#define	XHCI_TRB_ERROR_STREAM_TYPE	0x0A
+#define	XHCI_TRB_ERROR_SLOT_NOT_ON	0x0B
+#define	XHCI_TRB_ERROR_ENDP_NOT_ON	0x0C
+#define	XHCI_TRB_ERROR_SHORT_PKT	0x0D
+#define	XHCI_TRB_ERROR_RING_UNDERRUN	0x0E
+#define	XHCI_TRB_ERROR_RING_OVERRUN	0x0F
+#define	XHCI_TRB_ERROR_VF_RING_FULL	0x10
+#define	XHCI_TRB_ERROR_PARAMETER	0x11
+#define	XHCI_TRB_ERROR_BW_OVERRUN	0x12
+#define	XHCI_TRB_ERROR_CONTEXT_STATE	0x13
+#define	XHCI_TRB_ERROR_NO_PING_RESP	0x14
+#define	XHCI_TRB_ERROR_EV_RING_FULL	0x15
+#define	XHCI_TRB_ERROR_INCOMPAT_DEV	0x16
+#define	XHCI_TRB_ERROR_MISSED_SERVICE	0x17
+#define	XHCI_TRB_ERROR_CMD_RING_STOP	0x18
+#define	XHCI_TRB_ERROR_CMD_ABORTED	0x19
+#define	XHCI_TRB_ERROR_STOPPED		0x1A
+#define	XHCI_TRB_ERROR_LENGTH		0x1B
+#define	XHCI_TRB_ERROR_BAD_MELAT	0x1D
+#define	XHCI_TRB_ERROR_ISOC_OVERRUN	0x1F
+#define	XHCI_TRB_ERROR_EVENT_LOST	0x20
+#define	XHCI_TRB_ERROR_UNDEFINED	0x21
+#define	XHCI_TRB_ERROR_INVALID_SID	0x22
+#define	XHCI_TRB_ERROR_SEC_BW		0x23
+#define	XHCI_TRB_ERROR_SPLIT_XACT	0x24
+} __aligned(4);
+
+struct xhci_dev_endpoint_trbs {
+	struct xhci_trb		trb[(XHCI_MAX_STREAMS *
+	    XHCI_MAX_TRANSFERS) + XHCI_MAX_STREAMS];
+};
+
+struct xhci_event_ring_seg {
+	volatile uint64_t	qwEvrsTablePtr;
+	volatile uint32_t	dwEvrsTableSize;
+	volatile uint32_t	dwEvrsReserved;
+};
+
+#endif /* _PCI_XHCI_H_ */
diff --git a/ps2kbd.c b/ps2kbd.c
new file mode 100644
index 000000000000..aa4aeae03efd
--- /dev/null
+++ b/ps2kbd.c
@@ -0,0 +1,481 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * Copyright (c) 2015 Nahanni Systems Inc.
+ * 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 ``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>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <strings.h>
+#include <pthread.h>
+#include <pthread_np.h>
+
+#include "atkbdc.h"
+#include "console.h"
+
+/* keyboard device commands */
+#define	PS2KC_RESET_DEV		0xff
+#define	PS2KC_DISABLE		0xf5
+#define	PS2KC_ENABLE		0xf4
+#define	PS2KC_SET_TYPEMATIC	0xf3
+#define	PS2KC_SEND_DEV_ID	0xf2
+#define	PS2KC_SET_SCANCODE_SET	0xf0
+#define	PS2KC_ECHO		0xee
+#define	PS2KC_SET_LEDS		0xed
+
+#define	PS2KC_BAT_SUCCESS	0xaa
+#define	PS2KC_ACK		0xfa
+
+#define	PS2KBD_FIFOSZ		16
+
+struct fifo {
+	uint8_t	buf[PS2KBD_FIFOSZ];
+	int	rindex;		/* index to read from */
+	int	windex;		/* index to write to */
+	int	num;		/* number of bytes in the fifo */
+	int	size;		/* size of the fifo */
+};
+
+struct ps2kbd_softc {
+	struct atkbdc_softc	*atkbdc_sc;
+	pthread_mutex_t		mtx;
+
+	bool			enabled;
+	struct fifo		fifo;
+
+	uint8_t			curcmd;	/* current command for next byte */
+};
+
+static void
+fifo_init(struct ps2kbd_softc *sc)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	fifo->size = sizeof(((struct fifo *)0)->buf);
+}
+
+static void
+fifo_reset(struct ps2kbd_softc *sc)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	bzero(fifo, sizeof(struct fifo));
+	fifo->size = sizeof(((struct fifo *)0)->buf);
+}
+
+static int
+fifo_available(struct ps2kbd_softc *sc)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	return (fifo->num < fifo->size);
+}
+
+static void
+fifo_put(struct ps2kbd_softc *sc, uint8_t val)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	if (fifo->num < fifo->size) {
+		fifo->buf[fifo->windex] = val;
+		fifo->windex = (fifo->windex + 1) % fifo->size;
+		fifo->num++;
+	}
+}
+
+static int
+fifo_get(struct ps2kbd_softc *sc, uint8_t *val)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	if (fifo->num > 0) {
+		*val = fifo->buf[fifo->rindex];
+		fifo->rindex = (fifo->rindex + 1) % fifo->size;
+		fifo->num--;
+		return (0);
+	}
+
+	return (-1);
+}
+
+int
+ps2kbd_read(struct ps2kbd_softc *sc, uint8_t *val)
+{
+	int retval;
+
+	pthread_mutex_lock(&sc->mtx);
+	retval = fifo_get(sc, val);
+	pthread_mutex_unlock(&sc->mtx);
+
+	return (retval);
+}
+
+void
+ps2kbd_write(struct ps2kbd_softc *sc, uint8_t val)
+{
+	pthread_mutex_lock(&sc->mtx);
+	if (sc->curcmd) {
+		switch (sc->curcmd) {
+		case PS2KC_SET_TYPEMATIC:
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		case PS2KC_SET_SCANCODE_SET:
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		case PS2KC_SET_LEDS:
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		default:
+			fprintf(stderr, "Unhandled ps2 keyboard current "
+			    "command byte 0x%02x\n", val);
+			break;
+		}
+		sc->curcmd = 0;
+	} else {
+		switch (val) {
+		case 0x00:
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		case PS2KC_RESET_DEV:
+			fifo_reset(sc);
+			fifo_put(sc, PS2KC_ACK);
+			fifo_put(sc, PS2KC_BAT_SUCCESS);
+			break;
+		case PS2KC_DISABLE:
+			sc->enabled = false;
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		case PS2KC_ENABLE:
+			sc->enabled = true;
+			fifo_reset(sc);
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		case PS2KC_SET_TYPEMATIC:
+			sc->curcmd = val;
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		case PS2KC_SEND_DEV_ID:
+			fifo_put(sc, PS2KC_ACK);
+			fifo_put(sc, 0xab);
+			fifo_put(sc, 0x83);
+			break;
+		case PS2KC_SET_SCANCODE_SET:
+			sc->curcmd = val;
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		case PS2KC_ECHO:
+			fifo_put(sc, PS2KC_ECHO);
+			break;
+		case PS2KC_SET_LEDS:
+			sc->curcmd = val;
+			fifo_put(sc, PS2KC_ACK);
+			break;
+		default:
+			fprintf(stderr, "Unhandled ps2 keyboard command "
+			    "0x%02x\n", val);
+			break;
+		}
+	}
+	pthread_mutex_unlock(&sc->mtx);
+}
+
+/*
+ * Translate keysym to type 2 scancode and insert into keyboard buffer.
+ */
+static void
+ps2kbd_keysym_queue(struct ps2kbd_softc *sc,
+    int down, uint32_t keysym)
+{
+	/* ASCII to type 2 scancode lookup table */
+	const uint8_t translation[128] = {
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x29, 0x16, 0x52, 0x26, 0x25, 0x2e, 0x3d, 0x52,
+		0x46, 0x45, 0x3e, 0x55, 0x41, 0x4e, 0x49, 0x4a,
+		0x45, 0x16, 0x1e, 0x26, 0x25, 0x2e, 0x36, 0x3d,
+		0x3e, 0x46, 0x4c, 0x4c, 0x41, 0x55, 0x49, 0x4a,
+		0x1e, 0x1c, 0x32, 0x21, 0x23, 0x24, 0x2b, 0x34,
+		0x33, 0x43, 0x3b, 0x42, 0x4b, 0x3a, 0x31, 0x44,
+		0x4d, 0x15, 0x2d, 0x1b, 0x2c, 0x3c, 0x2a, 0x1d,
+		0x22, 0x35, 0x1a, 0x54, 0x5d, 0x5b, 0x36, 0x4e,
+		0x0e, 0x1c, 0x32, 0x21, 0x23, 0x24, 0x2b, 0x34,
+		0x33, 0x43, 0x3b, 0x42, 0x4b, 0x3a, 0x31, 0x44,
+		0x4d, 0x15, 0x2d, 0x1b, 0x2c, 0x3c, 0x2a, 0x1d,
+		0x22, 0x35, 0x1a, 0x54, 0x5d, 0x5b, 0x0e, 0x00,
+	};
+
+	assert(pthread_mutex_isowned_np(&sc->mtx));
+
+	switch (keysym) {
+	case 0x0 ... 0x7f:
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, translation[keysym]);
+		break;
+	case 0xff08:	/* Back space */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x66);
+		break;
+	case 0xff09:	/* Tab */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x0d);
+		break;
+	case 0xff0d:	/* Return  */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x5a);
+		break;
+	case 0xff1b:	/* Escape */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x76);
+		break;
+	case 0xff50:	/* Home */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x6c);
+		break;
+	case 0xff51:	/* Left arrow */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x6b);
+		break;
+	case 0xff52:	/* Up arrow */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x75);
+		break;
+	case 0xff53:	/* Right arrow */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x74);
+		break;
+	case 0xff54:	/* Down arrow */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x72);
+		break;
+	case 0xff55:	/* PgUp */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);	
+		fifo_put(sc, 0x7d);
+		break;
+	case 0xff56:	/* PgDwn */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);	
+		fifo_put(sc, 0x7a);
+		break;
+	case 0xff57:	/* End */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);	
+		fifo_put(sc, 0x69);
+		break;
+	case 0xff63:	/* Ins */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);	
+		fifo_put(sc, 0x70);
+		break;
+	case 0xff8d:	/* Keypad Enter */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x5a);
+		break;
+	case 0xffe1:	/* Left shift */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x12);
+		break;
+	case 0xffe2:	/* Right shift */
+		/* XXX */
+		break;
+	case 0xffe3:	/* Left control */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x14);
+		break;
+	case 0xffe4:	/* Right control */
+		/* XXX */
+		break;
+	case 0xffe7:	/* Left meta */
+		/* XXX */
+		break;
+	case 0xffe8:	/* Right meta */
+		/* XXX */
+		break;
+	case 0xffe9:	/* Left alt */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x11);
+		break;
+	case 0xffea:	/* Right alt */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x11);
+		break;
+	case 0xffeb:	/* Left Windows */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x1f);
+		break;
+	case 0xffec:	/* Right Windows */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x27);
+		break;
+	case 0xffbe:    /* F1 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x05);
+		break;
+	case 0xffbf:    /* F2 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x06);
+		break;
+	case 0xffc0:    /* F3 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x04);
+		break;
+	case 0xffc1:    /* F4 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x0C);
+		break;
+	case 0xffc2:    /* F5 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x03);
+		break;
+	case 0xffc3:    /* F6 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x0B);
+		break;
+	case 0xffc4:    /* F7 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x83);
+		break;
+	case 0xffc5:    /* F8 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x0A);
+		break;
+	case 0xffc6:    /* F9 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x01);
+		break;
+	case 0xffc7:    /* F10 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x09);
+		break;
+	case 0xffc8:    /* F11 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x78);
+		break;
+	case 0xffc9:    /* F12 */
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x07);
+		break;
+	case 0xffff:    /* Del */
+		fifo_put(sc, 0xe0);
+		if (!down)
+			fifo_put(sc, 0xf0);
+		fifo_put(sc, 0x71);
+		break;
+	default:
+		fprintf(stderr, "Unhandled ps2 keyboard keysym 0x%x\n",
+		     keysym);
+		break;
+	}
+}
+
+static void
+ps2kbd_event(int down, uint32_t keysym, void *arg)
+{
+	struct ps2kbd_softc *sc = arg;
+	int fifo_full;
+
+	pthread_mutex_lock(&sc->mtx);
+	if (!sc->enabled) {
+		pthread_mutex_unlock(&sc->mtx);
+		return;
+	}
+	fifo_full = sc->fifo.num == PS2KBD_FIFOSZ;
+	ps2kbd_keysym_queue(sc, down, keysym);
+	pthread_mutex_unlock(&sc->mtx);
+
+	if (!fifo_full)
+		atkbdc_event(sc->atkbdc_sc, 1);
+}
+
+struct ps2kbd_softc *
+ps2kbd_init(struct atkbdc_softc *atkbdc_sc)
+{
+	struct ps2kbd_softc *sc;
+
+	sc = calloc(1, sizeof (struct ps2kbd_softc));
+	pthread_mutex_init(&sc->mtx, NULL);
+	fifo_init(sc);
+	sc->atkbdc_sc = atkbdc_sc;
+
+	console_kbd_register(ps2kbd_event, sc, 1);
+
+	return (sc);
+}
+
diff --git a/ps2kbd.h b/ps2kbd.h
new file mode 100644
index 000000000000..34c31b1ea88e
--- /dev/null
+++ b/ps2kbd.h
@@ -0,0 +1,39 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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$
+ */
+
+#ifndef _PS2KBD_H_
+#define	_PS2KBD_H_
+
+struct atkbdc_softc;
+
+struct ps2kbd_softc *ps2kbd_init(struct atkbdc_softc *sc);
+
+int ps2kbd_read(struct ps2kbd_softc *sc, uint8_t *val);
+void ps2kbd_write(struct ps2kbd_softc *sc, uint8_t val);
+
+#endif /* _PS2KBD_H_ */
diff --git a/ps2mouse.c b/ps2mouse.c
new file mode 100644
index 000000000000..e207a939a37d
--- /dev/null
+++ b/ps2mouse.c
@@ -0,0 +1,405 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * Copyright (c) 2015 Nahanni Systems Inc.
+ * 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 ``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>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <strings.h>
+#include <pthread.h>
+#include <pthread_np.h>
+
+#include "atkbdc.h"
+#include "console.h"
+
+/* mouse device commands */
+#define	PS2MC_RESET_DEV		0xff
+#define	PS2MC_SET_DEFAULTS	0xf6
+#define	PS2MC_DISABLE		0xf5
+#define	PS2MC_ENABLE		0xf4
+#define	PS2MC_SET_SAMPLING_RATE	0xf3
+#define	PS2MC_SEND_DEV_ID	0xf2
+#define	PS2MC_SET_REMOTE_MODE	0xf0
+#define	PS2MC_SEND_DEV_DATA	0xeb
+#define	PS2MC_SET_STREAM_MODE	0xea
+#define	PS2MC_SEND_DEV_STATUS	0xe9
+#define	PS2MC_SET_RESOLUTION	0xe8
+#define	PS2MC_SET_SCALING1	0xe7
+#define	PS2MC_SET_SCALING2	0xe6
+
+#define	PS2MC_BAT_SUCCESS	0xaa
+#define	PS2MC_ACK		0xfa
+
+/* mouse device id */
+#define	PS2MOUSE_DEV_ID		0x0
+
+/* mouse status bits */
+#define	PS2M_STS_REMOTE_MODE	0x40
+#define	PS2M_STS_ENABLE_DEV	0x20
+#define	PS2M_STS_SCALING_21	0x10
+#define	PS2M_STS_MID_BUTTON	0x04
+#define	PS2M_STS_RIGHT_BUTTON	0x02
+#define	PS2M_STS_LEFT_BUTTON	0x01
+
+#define	PS2MOUSE_FIFOSZ		16
+
+struct fifo {
+	uint8_t	buf[PS2MOUSE_FIFOSZ];
+	int	rindex;		/* index to read from */
+	int	windex;		/* index to write to */
+	int	num;		/* number of bytes in the fifo */
+	int	size;		/* size of the fifo */
+};
+
+struct ps2mouse_softc {
+	struct atkbdc_softc	*atkbdc_sc;
+	pthread_mutex_t		mtx;
+
+	uint8_t		status;
+	uint8_t		resolution;
+	uint8_t		sampling_rate;
+	int		ctrlenable;
+	struct fifo	fifo;
+
+	uint8_t		curcmd;	/* current command for next byte */
+
+	int		cur_x, cur_y;
+	int		delta_x, delta_y;
+};
+
+static void
+fifo_init(struct ps2mouse_softc *sc)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	fifo->size = sizeof(((struct fifo *)0)->buf);
+}
+
+static void
+fifo_reset(struct ps2mouse_softc *sc)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	bzero(fifo, sizeof(struct fifo));
+	fifo->size = sizeof(((struct fifo *)0)->buf);
+}
+
+static void
+fifo_put(struct ps2mouse_softc *sc, uint8_t val)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	if (fifo->num < fifo->size) {
+		fifo->buf[fifo->windex] = val;
+		fifo->windex = (fifo->windex + 1) % fifo->size;
+		fifo->num++;
+	}
+}
+
+static int
+fifo_get(struct ps2mouse_softc *sc, uint8_t *val)
+{
+	struct fifo *fifo;
+
+	fifo = &sc->fifo;
+	if (fifo->num > 0) {
+		*val = fifo->buf[fifo->rindex];
+		fifo->rindex = (fifo->rindex + 1) % fifo->size;
+		fifo->num--;
+		return (0);
+	}
+
+	return (-1);
+}
+
+static void
+movement_reset(struct ps2mouse_softc *sc)
+{
+	assert(pthread_mutex_isowned_np(&sc->mtx));
+
+	sc->delta_x = 0;
+	sc->delta_y = 0;
+}
+
+static void
+movement_update(struct ps2mouse_softc *sc, int x, int y)
+{
+	sc->delta_x += x - sc->cur_x;
+	sc->delta_y += sc->cur_y - y;
+	sc->cur_x = x;
+	sc->cur_y = y;
+}
+
+static void
+movement_get(struct ps2mouse_softc *sc)
+{
+	uint8_t val0, val1, val2;
+
+	assert(pthread_mutex_isowned_np(&sc->mtx));
+
+	val0 = 	sc->status & (PS2M_STS_LEFT_BUTTON |
+	    PS2M_STS_RIGHT_BUTTON | PS2M_STS_MID_BUTTON);
+
+	if (sc->delta_x >= 0) {
+		if (sc->delta_x > 255) {
+			val0 |= (1 << 6);
+			val1 = 255;
+		} else
+			val1 = sc->delta_x;
+	} else {
+		val0 |= (1 << 4);
+		if (sc->delta_x < -255) {
+			val0 |= (1 << 6);
+			val1 = 255;
+		} else
+			val1 = sc->delta_x;
+	}
+	sc->delta_x = 0;
+
+	if (sc->delta_y >= 0) {
+		if (sc->delta_y > 255) {
+			val0 |= (1 << 7);
+			val2 = 255;
+		} else
+			val2 = sc->delta_y;
+	} else {
+		val0 |= (1 << 5);
+		if (sc->delta_y < -255) {
+			val0 |= (1 << 7);
+			val2 = 255;
+		} else
+			val2 = sc->delta_y;
+	}
+	sc->delta_y = 0;
+
+	if (sc->fifo.num < (sc->fifo.size - 3)) {
+		fifo_put(sc, val0);
+		fifo_put(sc, val1);
+		fifo_put(sc, val2);
+	}
+}
+
+static void
+ps2mouse_reset(struct ps2mouse_softc *sc)
+{
+	assert(pthread_mutex_isowned_np(&sc->mtx));
+	fifo_reset(sc);
+	movement_reset(sc);
+	sc->status = PS2M_STS_ENABLE_DEV;
+	sc->resolution = 4;
+	sc->sampling_rate = 100;
+
+	sc->cur_x = 0;
+	sc->cur_y = 0;
+	sc->delta_x = 0;
+	sc->delta_y = 0;
+}
+
+int
+ps2mouse_read(struct ps2mouse_softc *sc, uint8_t *val)
+{
+	int retval;
+
+	pthread_mutex_lock(&sc->mtx);
+	retval = fifo_get(sc, val);
+	pthread_mutex_unlock(&sc->mtx);
+
+	return (retval);
+}
+
+int
+ps2mouse_fifocnt(struct ps2mouse_softc *sc)
+{
+	return (sc->fifo.num);
+}
+
+void
+ps2mouse_toggle(struct ps2mouse_softc *sc, int enable)
+{
+	pthread_mutex_lock(&sc->mtx);
+	if (enable)
+		sc->ctrlenable = 1;
+	else {
+		sc->ctrlenable = 0;
+		sc->fifo.rindex = 0;
+		sc->fifo.windex = 0;
+		sc->fifo.num = 0;
+	}
+	pthread_mutex_unlock(&sc->mtx);
+}
+
+void
+ps2mouse_write(struct ps2mouse_softc *sc, uint8_t val, int insert)
+{
+	pthread_mutex_lock(&sc->mtx);
+	fifo_reset(sc);
+	if (sc->curcmd) {
+		switch (sc->curcmd) {
+		case PS2MC_SET_SAMPLING_RATE:
+			sc->sampling_rate = val;
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_SET_RESOLUTION:
+			sc->resolution = val;
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		default:
+			fprintf(stderr, "Unhandled ps2 mouse current "
+			    "command byte 0x%02x\n", val);
+			break;
+		}
+		sc->curcmd = 0;
+
+	} else if (insert) {
+		fifo_put(sc, val);
+	} else {
+		switch (val) {
+		case 0x00:
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_RESET_DEV:
+			ps2mouse_reset(sc);
+			fifo_put(sc, PS2MC_ACK);
+			fifo_put(sc, PS2MC_BAT_SUCCESS);
+			fifo_put(sc, PS2MOUSE_DEV_ID);
+			break;
+		case PS2MC_SET_DEFAULTS:
+			ps2mouse_reset(sc);
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_DISABLE:
+			fifo_reset(sc);
+			sc->status &= ~PS2M_STS_ENABLE_DEV;
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_ENABLE:
+			fifo_reset(sc);
+			sc->status |= PS2M_STS_ENABLE_DEV;
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_SET_SAMPLING_RATE:
+			sc->curcmd = val;
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_SEND_DEV_ID:
+			fifo_put(sc, PS2MC_ACK);
+			fifo_put(sc, PS2MOUSE_DEV_ID);
+			break;
+		case PS2MC_SET_REMOTE_MODE:
+			sc->status |= PS2M_STS_REMOTE_MODE;
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_SEND_DEV_DATA:
+			fifo_put(sc, PS2MC_ACK);
+			movement_get(sc);
+			break;
+		case PS2MC_SET_STREAM_MODE:
+			sc->status &= ~PS2M_STS_REMOTE_MODE;
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_SEND_DEV_STATUS:
+			fifo_put(sc, PS2MC_ACK);
+			fifo_put(sc, sc->status);
+			fifo_put(sc, sc->resolution);
+			fifo_put(sc, sc->sampling_rate);
+			break;
+		case PS2MC_SET_RESOLUTION:
+			sc->curcmd = val;
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		case PS2MC_SET_SCALING1:
+		case PS2MC_SET_SCALING2:
+			fifo_put(sc, PS2MC_ACK);
+			break;
+		default:
+			fifo_put(sc, PS2MC_ACK);
+			fprintf(stderr, "Unhandled ps2 mouse command "
+			    "0x%02x\n", val);
+			break;
+		}
+	}
+	pthread_mutex_unlock(&sc->mtx);
+}
+
+static void
+ps2mouse_event(uint8_t button, int x, int y, void *arg)
+{
+	struct ps2mouse_softc *sc = arg;
+
+	pthread_mutex_lock(&sc->mtx);
+	movement_update(sc, x, y);
+
+	sc->status &= ~(PS2M_STS_LEFT_BUTTON |
+	    PS2M_STS_RIGHT_BUTTON | PS2M_STS_MID_BUTTON);
+	if (button & (1 << 0))
+		sc->status |= PS2M_STS_LEFT_BUTTON;
+	if (button & (1 << 1))
+		sc->status |= PS2M_STS_MID_BUTTON;
+	if (button & (1 << 2))
+		sc->status |= PS2M_STS_RIGHT_BUTTON;
+
+	if ((sc->status & PS2M_STS_ENABLE_DEV) == 0 || !sc->ctrlenable) {
+		/* no data reporting */
+		pthread_mutex_unlock(&sc->mtx);
+		return;
+	}
+
+	movement_get(sc);
+	pthread_mutex_unlock(&sc->mtx);
+
+	if (sc->fifo.num > 0)
+		atkbdc_event(sc->atkbdc_sc, 0);
+}
+
+struct ps2mouse_softc *
+ps2mouse_init(struct atkbdc_softc *atkbdc_sc)
+{
+	struct ps2mouse_softc *sc;
+
+	sc = calloc(1, sizeof (struct ps2mouse_softc));
+	pthread_mutex_init(&sc->mtx, NULL);
+	fifo_init(sc);
+	sc->atkbdc_sc = atkbdc_sc;
+
+	pthread_mutex_lock(&sc->mtx);
+	ps2mouse_reset(sc);
+	pthread_mutex_unlock(&sc->mtx);
+
+	console_ptr_register(ps2mouse_event, sc, 1);
+
+	return (sc);
+}
+
+
diff --git a/ps2mouse.h b/ps2mouse.h
new file mode 100644
index 000000000000..10d5698a3085
--- /dev/null
+++ b/ps2mouse.h
@@ -0,0 +1,41 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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$
+ */
+
+#ifndef _PS2MOUSE_H_
+#define	_PS2MOUSE_H_
+
+struct atkbdc_softc;
+
+struct ps2mouse_softc *ps2mouse_init(struct atkbdc_softc *sc);
+
+int ps2mouse_read(struct ps2mouse_softc *sc, uint8_t *val);
+void ps2mouse_write(struct ps2mouse_softc *sc, uint8_t val, int insert);
+void ps2mouse_toggle(struct ps2mouse_softc *sc, int enable);
+int ps2mouse_fifocnt(struct ps2mouse_softc *sc);
+
+#endif /* _PS2MOUSE_H_ */
diff --git a/rfb.c b/rfb.c
new file mode 100644
index 000000000000..ceeb1b754502
--- /dev/null
+++ b/rfb.c
@@ -0,0 +1,949 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * Copyright (c) 2015 Leon Dang
+ * 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 ``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>
+__FBSDID("$FreeBSD$");
+
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <sys/select.h>
+#include <sys/param.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#include <assert.h>
+#include <pthread.h>
+#include <pthread_np.h>
+#include <signal.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <zlib.h>
+#include <cpuid.h>
+
+#include "bhyvegc.h"
+#include "console.h"
+#include "rfb.h"
+#include "sockstream.h"
+
+static int rfb_debug = 0;
+#define	DPRINTF(params) if (rfb_debug) printf params
+#define	WPRINTF(params) printf params
+
+struct rfb_softc {
+	int		sfd;
+	pthread_t	tid;
+
+	int		cfd;
+
+	int		width, height;
+
+	bool		enc_raw_ok;
+	bool		enc_zlib_ok;
+	bool		enc_resize_ok;
+
+	z_stream	zstream;
+	uint8_t		*zbuf;
+	int		zbuflen;
+
+	int		conn_wait;
+	int		sending;
+	pthread_mutex_t mtx;
+	pthread_cond_t  cond;
+
+	int		hw_crc;
+	uint32_t	*crc;		/* WxH crc cells */
+	uint32_t	*crc_tmp;	/* buffer to store single crc row */
+	int		crc_width, crc_height;
+};
+
+struct rfb_pixfmt {
+	uint8_t		bpp;
+	uint8_t		depth;
+	uint8_t		bigendian;
+	uint8_t		truecolor;
+	uint16_t	red_max;
+	uint16_t	green_max;
+	uint16_t	blue_max;
+	uint8_t		red_shift;
+	uint8_t		green_shift;
+	uint8_t		blue_shift;
+	uint8_t		pad[3];
+};
+
+struct rfb_srvr_info {
+	uint16_t		width;
+	uint16_t		height;
+	struct rfb_pixfmt	pixfmt;
+	uint32_t		namelen;
+};
+
+struct rfb_pixfmt_msg {
+	uint8_t			type;
+	uint8_t			pad[3];
+	struct rfb_pixfmt	pixfmt;
+};
+
+#define	RFB_ENCODING_RAW		0
+#define	RFB_ENCODING_ZLIB		6
+#define	RFB_ENCODING_RESIZE		-223
+
+#define RFB_MAX_WIDTH			2000
+#define RFB_MAX_HEIGHT			1200
+#define	RFB_ZLIB_BUFSZ			RFB_MAX_WIDTH*RFB_MAX_HEIGHT*4
+
+/* percentage changes to screen before sending the entire screen */
+#define RFB_SEND_ALL_THRESH             25
+
+struct rfb_enc_msg {
+	uint8_t		type;
+	uint8_t		pad;
+	uint16_t	numencs;
+};
+
+struct rfb_updt_msg {
+	uint8_t		type;
+	uint8_t		incremental;
+	uint16_t	x;
+	uint16_t	y;
+	uint16_t	width;
+	uint16_t	height;
+};
+
+struct rfb_key_msg {
+	uint8_t		type;
+	uint8_t		down;
+	uint16_t	pad;
+	uint32_t	code;
+};
+
+struct rfb_ptr_msg {
+	uint8_t		type;
+	uint8_t		button;
+	uint16_t	x;
+	uint16_t	y;
+};
+
+struct rfb_srvr_updt_msg {
+	uint8_t		type;
+	uint8_t		pad;
+	uint16_t	numrects;
+};
+
+struct rfb_srvr_rect_hdr {
+	uint16_t	x;
+	uint16_t	y;
+	uint16_t	width;
+	uint16_t	height;
+	uint32_t	encoding;
+};
+
+struct rfb_cuttext_msg {
+	uint8_t		type;
+	uint8_t		padding[3];
+	uint32_t	length;
+};
+
+
+static void
+rfb_send_server_init_msg(int cfd)
+{
+	struct bhyvegc_image *gc_image;
+	struct rfb_srvr_info sinfo;
+	int len;
+
+	gc_image = console_get_image();
+
+	sinfo.width = htons(gc_image->width);
+	sinfo.height = htons(gc_image->height);
+	sinfo.pixfmt.bpp = 32;
+	sinfo.pixfmt.depth = 32;
+	sinfo.pixfmt.bigendian = 0;
+	sinfo.pixfmt.truecolor = 1;
+	sinfo.pixfmt.red_max = htons(255);
+	sinfo.pixfmt.green_max = htons(255);
+	sinfo.pixfmt.blue_max = htons(255);
+	sinfo.pixfmt.red_shift = 16;
+	sinfo.pixfmt.green_shift = 8;
+	sinfo.pixfmt.blue_shift = 0;
+	sinfo.namelen = htonl(strlen("bhyve"));
+	len = stream_write(cfd, &sinfo, sizeof(sinfo));
+	len = stream_write(cfd, "bhyve", strlen("bhyve"));
+}
+
+static void
+rfb_send_resize_update_msg(struct rfb_softc *rc, int cfd)
+{
+	struct rfb_srvr_updt_msg supdt_msg;
+        struct rfb_srvr_rect_hdr srect_hdr;
+
+	/* Number of rectangles: 1 */
+	supdt_msg.type = 0;
+	supdt_msg.pad = 0;
+	supdt_msg.numrects = htons(1);
+	stream_write(cfd, &supdt_msg, sizeof(struct rfb_srvr_updt_msg));
+
+	/* Rectangle header */
+	srect_hdr.x = htons(0);
+	srect_hdr.y = htons(0);
+	srect_hdr.width = htons(rc->width);
+	srect_hdr.height = htons(rc->height);
+	srect_hdr.encoding = htonl(RFB_ENCODING_RESIZE);
+	stream_write(cfd, &srect_hdr, sizeof(struct rfb_srvr_rect_hdr));
+}
+
+static void
+rfb_recv_set_pixfmt_msg(struct rfb_softc *rc, int cfd)
+{
+	struct rfb_pixfmt_msg pixfmt_msg;
+	int len;
+
+	len = stream_read(cfd, ((void *)&pixfmt_msg)+1, sizeof(pixfmt_msg)-1);
+}
+
+
+static void
+rfb_recv_set_encodings_msg(struct rfb_softc *rc, int cfd)
+{
+	struct rfb_enc_msg enc_msg;
+	int len, i;
+	uint32_t encoding;
+
+	assert((sizeof(enc_msg) - 1) == 3);
+	len = stream_read(cfd, ((void *)&enc_msg)+1, sizeof(enc_msg)-1);
+
+	for (i = 0; i < htons(enc_msg.numencs); i++) {
+		len = stream_read(cfd, &encoding, sizeof(encoding));
+		switch (htonl(encoding)) {
+		case RFB_ENCODING_RAW:
+			rc->enc_raw_ok = true;
+			break;
+		case RFB_ENCODING_ZLIB:
+			rc->enc_zlib_ok = true;
+			deflateInit(&rc->zstream, Z_BEST_SPEED);
+			break;
+		case RFB_ENCODING_RESIZE:
+			rc->enc_resize_ok = true;
+			break;
+		}
+	}
+}
+
+static void
+rfb_resize_update(struct rfb_softc *rc, int fd)
+{
+	struct rfb_srvr_updt_msg supdt_msg;
+        struct rfb_srvr_rect_hdr srect_hdr;
+
+	/* Number of rectangles: 1 */
+	supdt_msg.type = 0;
+	supdt_msg.pad = 0;
+	supdt_msg.numrects = htons(1);
+	stream_write(fd, &supdt_msg, sizeof(struct rfb_srvr_updt_msg));
+
+	/* Rectangle header */
+	srect_hdr.x = htons(0);
+	srect_hdr.y = htons(0);
+	srect_hdr.width = htons(rc->width);
+	srect_hdr.height = htons(rc->height);
+	srect_hdr.encoding = htonl(RFB_ENCODING_RESIZE);
+	stream_write(fd, &srect_hdr, sizeof(struct rfb_srvr_rect_hdr));
+}
+
+/*
+ * Calculate CRC32 using SSE4.2; Intel or AMD Bulldozer+ CPUs only
+ */
+static __inline uint32_t
+fast_crc32(void *buf, int len, uint32_t crcval)
+{
+	uint32_t q = len / sizeof(uint32_t);
+	uint32_t *p = (uint32_t *)buf;
+
+	while (q--) {
+		asm volatile (
+			".byte 0xf2, 0xf, 0x38, 0xf1, 0xf1;"
+			:"=S" (crcval)
+			:"0" (crcval), "c" (*p)
+		);
+		p++;
+	}
+
+	return (crcval);
+}
+
+
+static int
+rfb_send_rect(struct rfb_softc *rc, int cfd, struct bhyvegc_image *gc,
+              int x, int y, int w, int h)
+{
+	struct rfb_srvr_updt_msg supdt_msg;
+        struct rfb_srvr_rect_hdr srect_hdr;
+	unsigned long zlen;
+	ssize_t nwrite, total;
+	int err;
+	uint32_t *p;
+	uint8_t *zbufp;
+
+	/*
+	 * Send a single rectangle of the given x, y, w h dimensions.
+	 */
+
+	/* Number of rectangles: 1 */
+	supdt_msg.type = 0;
+	supdt_msg.pad = 0;
+	supdt_msg.numrects = htons(1);
+	nwrite = stream_write(cfd, &supdt_msg,
+	                      sizeof(struct rfb_srvr_updt_msg));
+	if (nwrite <= 0)
+		return (nwrite);
+
+
+	/* Rectangle header */
+	srect_hdr.x = htons(x);
+	srect_hdr.y = htons(y);
+	srect_hdr.width = htons(w);
+	srect_hdr.height = htons(h);
+
+	h = y + h;
+	w *= sizeof(uint32_t);
+	if (rc->enc_zlib_ok) {
+		zbufp = rc->zbuf;
+		rc->zstream.total_in = 0;
+		rc->zstream.total_out = 0;
+		for (p = &gc->data[y * gc->width + x]; y < h; y++) {
+			rc->zstream.next_in = (Bytef *)p;
+			rc->zstream.avail_in = w;
+			rc->zstream.next_out = (Bytef *)zbufp;
+			rc->zstream.avail_out = RFB_ZLIB_BUFSZ + 16 -
+			                        rc->zstream.total_out;
+			rc->zstream.data_type = Z_BINARY;
+
+			/* Compress with zlib */
+			err = deflate(&rc->zstream, Z_SYNC_FLUSH);
+			if (err != Z_OK) {
+				WPRINTF(("zlib[rect] deflate err: %d\n", err));
+				rc->enc_zlib_ok = false;
+				deflateEnd(&rc->zstream);
+				goto doraw;
+			}
+			zbufp = rc->zbuf + rc->zstream.total_out;
+			p += gc->width;
+		}
+		srect_hdr.encoding = htonl(RFB_ENCODING_ZLIB);
+		nwrite = stream_write(cfd, &srect_hdr,
+		                      sizeof(struct rfb_srvr_rect_hdr));
+		if (nwrite <= 0)
+			return (nwrite);
+
+		zlen = htonl(rc->zstream.total_out);
+		nwrite = stream_write(cfd, &zlen, sizeof(uint32_t));
+		if (nwrite <= 0)
+			return (nwrite);
+		return (stream_write(cfd, rc->zbuf, rc->zstream.total_out));
+	}
+
+doraw:
+
+	total = 0;
+	zbufp = rc->zbuf;
+	for (p = &gc->data[y * gc->width + x]; y < h; y++) {
+		memcpy(zbufp, p, w);
+		zbufp += w;
+		total += w;
+		p += gc->width;
+	}
+
+	srect_hdr.encoding = htonl(RFB_ENCODING_RAW);
+	nwrite = stream_write(cfd, &srect_hdr,
+	                      sizeof(struct rfb_srvr_rect_hdr));
+	if (nwrite <= 0)
+		return (nwrite);
+
+	total = stream_write(cfd, rc->zbuf, total);
+
+	return (total);
+}
+
+static int
+rfb_send_all(struct rfb_softc *rc, int cfd, struct bhyvegc_image *gc)
+{
+	struct rfb_srvr_updt_msg supdt_msg;
+        struct rfb_srvr_rect_hdr srect_hdr;
+	ssize_t nwrite;
+	unsigned long zlen;
+	int err;
+
+	/*
+	 * Send the whole thing
+	 */
+
+	/* Number of rectangles: 1 */
+	supdt_msg.type = 0;
+	supdt_msg.pad = 0;
+	supdt_msg.numrects = htons(1);
+	nwrite = stream_write(cfd, &supdt_msg,
+	                      sizeof(struct rfb_srvr_updt_msg));
+	if (nwrite <= 0)
+		return (nwrite);
+
+	/* Rectangle header */
+	srect_hdr.x = 0;
+	srect_hdr.y = 0;
+	srect_hdr.width = htons(gc->width);
+	srect_hdr.height = htons(gc->height);
+	if (rc->enc_zlib_ok) {
+		rc->zstream.next_in = (Bytef *)gc->data;
+		rc->zstream.avail_in = gc->width * gc->height *
+		                   sizeof(uint32_t);
+		rc->zstream.next_out = (Bytef *)rc->zbuf;
+		rc->zstream.avail_out = RFB_ZLIB_BUFSZ + 16;
+		rc->zstream.data_type = Z_BINARY;
+
+		rc->zstream.total_in = 0;
+		rc->zstream.total_out = 0;
+
+		/* Compress with zlib */
+		err = deflate(&rc->zstream, Z_SYNC_FLUSH);
+		if (err != Z_OK) {
+			WPRINTF(("zlib deflate err: %d\n", err));
+			rc->enc_zlib_ok = false;
+			deflateEnd(&rc->zstream);
+			goto doraw;
+		}
+
+		srect_hdr.encoding = htonl(RFB_ENCODING_ZLIB);
+		nwrite = stream_write(cfd, &srect_hdr,
+		                      sizeof(struct rfb_srvr_rect_hdr));
+		if (nwrite <= 0)
+			return (nwrite);
+
+		zlen = htonl(rc->zstream.total_out);
+		nwrite = stream_write(cfd, &zlen, sizeof(uint32_t));
+		if (nwrite <= 0)
+			return (nwrite);
+		return (stream_write(cfd, rc->zbuf, rc->zstream.total_out));
+	}
+
+doraw:
+	srect_hdr.encoding = htonl(RFB_ENCODING_RAW);
+	nwrite = stream_write(cfd, &srect_hdr,
+	                      sizeof(struct rfb_srvr_rect_hdr));
+	if (nwrite <= 0)
+		return (nwrite);
+
+	nwrite = stream_write(cfd, gc->data,
+	               gc->width * gc->height * sizeof(uint32_t));
+
+	return (nwrite);
+}
+
+#define PIX_PER_CELL	32
+#define	PIXCELL_SHIFT	5
+#define PIXCELL_MASK	0x1F
+
+static int
+rfb_send_screen(struct rfb_softc *rc, int cfd, int all)
+{
+	struct bhyvegc_image *gc_image;
+	ssize_t nwrite;
+	int x, y;
+	int celly, cellwidth;
+	int xcells, ycells;
+	int w, h;
+	uint32_t *p;
+	int rem_x, rem_y;   /* remainder for resolutions not x32 pixels ratio */
+	int retval;
+	uint32_t *crc_p, *orig_crc;
+	int changes;
+
+	console_refresh();
+	gc_image = console_get_image();
+
+	pthread_mutex_lock(&rc->mtx);
+	if (rc->sending) {
+		pthread_mutex_unlock(&rc->mtx);
+		return (1);
+	}
+	rc->sending = 1;
+	pthread_mutex_unlock(&rc->mtx);
+
+	retval = 0;
+
+	if (all) {
+		retval = rfb_send_all(rc, cfd, gc_image);
+		goto done;
+	}
+
+	/*
+	 * Calculate the checksum for each 32x32 cell. Send each that
+	 * has changed since the last scan.
+	 */
+
+	/* Resolution changed */
+
+	rc->crc_width = gc_image->width;
+	rc->crc_height = gc_image->height;
+
+	w = rc->crc_width;
+	h = rc->crc_height;
+	xcells = howmany(rc->crc_width, PIX_PER_CELL);
+	ycells = howmany(rc->crc_height, PIX_PER_CELL);
+
+	rem_x = w & PIXCELL_MASK;
+
+	rem_y = h & PIXCELL_MASK;
+	if (!rem_y)
+		rem_y = PIX_PER_CELL;
+
+	p = gc_image->data;
+
+	/*
+	 * Go through all cells and calculate crc. If significant number
+	 * of changes, then send entire screen.
+	 * crc_tmp is dual purpose: to store the new crc and to flag as
+	 * a cell that has changed.
+	 */
+	crc_p = rc->crc_tmp - xcells;
+	orig_crc = rc->crc - xcells;
+	changes = 0;
+	memset(rc->crc_tmp, 0, sizeof(uint32_t) * xcells * ycells);
+	for (y = 0; y < h; y++) {
+		if ((y & PIXCELL_MASK) == 0) {
+			crc_p += xcells;
+			orig_crc += xcells;
+		}
+
+		for (x = 0; x < xcells; x++) {
+			if (rc->hw_crc)
+				crc_p[x] = fast_crc32(p,
+				             PIX_PER_CELL * sizeof(uint32_t),
+				             crc_p[x]);
+			else
+				crc_p[x] = (uint32_t)crc32(crc_p[x],
+				             (Bytef *)p,
+				             PIX_PER_CELL * sizeof(uint32_t));
+
+			p += PIX_PER_CELL;
+
+			/* check for crc delta if last row in cell */
+			if ((y & PIXCELL_MASK) == PIXCELL_MASK || y == (h-1)) {
+				if (orig_crc[x] != crc_p[x]) {
+					orig_crc[x] = crc_p[x];
+					crc_p[x] = 1;
+					changes++;
+				} else {
+					crc_p[x] = 0;
+				}
+			}
+		}
+
+		if (rem_x) {
+			if (rc->hw_crc)
+				crc_p[x] = fast_crc32(p,
+				                    rem_x * sizeof(uint32_t),
+				                    crc_p[x]);
+			else
+				crc_p[x] = (uint32_t)crc32(crc_p[x],
+				                    (Bytef *)p,
+				                    rem_x * sizeof(uint32_t));
+			p += rem_x;
+
+			if ((y & PIXCELL_MASK) == PIXCELL_MASK || y == (h-1)) {
+				if (orig_crc[x] != crc_p[x]) {
+					orig_crc[x] = crc_p[x];
+					crc_p[x] = 1;
+					changes++;
+				} else {
+					crc_p[x] = 0;
+				}
+			}
+		}
+	}
+
+	/* If number of changes is > THRESH percent, send the whole screen */
+	if (((changes * 100) / (xcells * ycells)) >= RFB_SEND_ALL_THRESH) {
+		retval = rfb_send_all(rc, cfd, gc_image);
+		goto done;
+	}
+	
+	/* Go through all cells, and send only changed ones */
+	crc_p = rc->crc_tmp;
+	for (y = 0; y < h; y += PIX_PER_CELL) {
+		/* previous cell's row */
+		celly = (y >> PIXCELL_SHIFT);
+
+		/* Delta check crc to previous set */
+		for (x = 0; x < xcells; x++) {
+			if (*crc_p++ == 0)
+				continue;
+
+			if (x == (xcells - 1) && rem_x > 0)
+				cellwidth = rem_x;
+			else
+				cellwidth = PIX_PER_CELL;
+			nwrite = rfb_send_rect(rc, cfd,
+				gc_image,
+				x * PIX_PER_CELL,
+				celly * PIX_PER_CELL,
+			        cellwidth,
+				y + PIX_PER_CELL >= h ? rem_y : PIX_PER_CELL);
+			if (nwrite <= 0) {
+				retval = nwrite;
+				goto done;
+			}
+		}
+	}
+	retval = 1;
+
+done:
+	pthread_mutex_lock(&rc->mtx);
+	rc->sending = 0;
+	pthread_mutex_unlock(&rc->mtx);
+	
+	return (retval);
+}
+
+
+static void
+rfb_recv_update_msg(struct rfb_softc *rc, int cfd, int discardonly)
+{
+	struct rfb_updt_msg updt_msg;
+	struct bhyvegc_image *gc_image;
+	int len;
+
+	len = stream_read(cfd, ((void *)&updt_msg) + 1 , sizeof(updt_msg) - 1);
+
+	console_refresh();
+	gc_image = console_get_image();
+
+	updt_msg.x = htons(updt_msg.x);
+	updt_msg.y = htons(updt_msg.y);
+	updt_msg.width = htons(updt_msg.width);
+	updt_msg.height = htons(updt_msg.height);
+
+	if (updt_msg.width != gc_image->width ||
+	    updt_msg.height != gc_image->height) {
+		rc->width = gc_image->width;
+		rc->height = gc_image->height;
+		if (rc->enc_resize_ok)
+			rfb_send_resize_update_msg(rc, cfd);
+	}
+
+	if (discardonly)
+		return;
+
+	rfb_send_screen(rc, cfd, 1);
+}
+
+static void
+rfb_recv_key_msg(struct rfb_softc *rc, int cfd)
+{
+	struct rfb_key_msg key_msg;
+	int len;
+
+	len = stream_read(cfd, ((void *)&key_msg) + 1, sizeof(key_msg) - 1);
+
+	console_key_event(key_msg.down, htonl(key_msg.code));
+}
+
+static void
+rfb_recv_ptr_msg(struct rfb_softc *rc, int cfd)
+{
+	struct rfb_ptr_msg ptr_msg;
+	int len;
+
+	len = stream_read(cfd, ((void *)&ptr_msg) + 1, sizeof(ptr_msg) - 1);
+
+	console_ptr_event(ptr_msg.button, htons(ptr_msg.x), htons(ptr_msg.y));
+}
+
+static void
+rfb_recv_cuttext_msg(struct rfb_softc *rc, int cfd)
+{
+	struct rfb_cuttext_msg ct_msg;
+	unsigned char buf[32];
+	int len;
+
+	len = stream_read(cfd, ((void *)&ct_msg) + 1, sizeof(ct_msg) - 1);
+	ct_msg.length = htonl(ct_msg.length);
+	while (ct_msg.length > 0) {
+		len = stream_read(cfd, buf, ct_msg.length > sizeof(buf) ?
+			sizeof(buf) : ct_msg.length);
+		ct_msg.length -= len;
+	}
+}
+
+static int64_t
+timeval_delta(struct timeval *prev, struct timeval *now)
+{
+	int64_t n1, n2;
+	n1 = now->tv_sec * 1000000 + now->tv_usec;
+	n2 = prev->tv_sec * 1000000 + prev->tv_usec;
+	return (n1 - n2);
+}
+
+static void *
+rfb_wr_thr(void *arg)
+{
+	struct rfb_softc *rc;
+	fd_set rfds;
+	struct timeval tv;
+	struct timeval prev_tv;
+	int64_t tdiff;
+	int cfd;
+	int err;
+
+	rc = arg;
+	cfd = rc->cfd;
+
+	prev_tv.tv_sec = 0;
+	prev_tv.tv_usec = 0;
+	while (rc->cfd >= 0) {
+		FD_ZERO(&rfds);
+		FD_SET(cfd, &rfds);
+		tv.tv_sec = 0;
+		tv.tv_usec = 10000;
+
+		err = select(cfd+1, &rfds, NULL, NULL, &tv);
+                if (err < 0)
+			return (NULL);
+
+		/* Determine if its time to push screen; ~24hz */
+		gettimeofday(&tv, NULL);
+		tdiff = timeval_delta(&prev_tv, &tv);
+		if (tdiff > 40000) {
+			prev_tv.tv_sec = tv.tv_sec;
+			prev_tv.tv_usec = tv.tv_usec;
+			if (rfb_send_screen(rc, cfd, 0) <= 0) {
+				return (NULL);
+			}
+		} else {
+			/* sleep */
+			usleep(40000 - tdiff);
+		}
+	}
+
+	return (NULL);
+}
+
+void
+rfb_handle(struct rfb_softc *rc, int cfd)
+{
+	const char *vbuf = "RFB 003.008\n";
+	unsigned char buf[80];
+	pthread_t tid;
+        uint32_t sres;
+	int len;
+
+	rc->cfd = cfd;
+
+	/* 1a. Send server version */
+	stream_write(cfd, vbuf, strlen(vbuf));
+
+	/* 1b. Read client version */
+	len = read(cfd, buf, sizeof(buf));
+
+	/* 2a. Send security type 'none' */
+	buf[0] = 1;
+	buf[1] = 1; /* none */
+	stream_write(cfd, buf, 2);
+
+
+	/* 2b. Read agreed security type */
+	len = stream_read(cfd, buf, 1);
+
+	/* 2c. Write back a status of 0 */
+	sres = 0;
+	stream_write(cfd, &sres, 4);
+
+	/* 3a. Read client shared-flag byte */
+	len = stream_read(cfd, buf, 1);
+
+	/* 4a. Write server-init info */
+	rfb_send_server_init_msg(cfd);
+
+	if (!rc->zbuf) {
+		rc->zbuf = malloc(RFB_ZLIB_BUFSZ + 16);
+		assert(rc->zbuf != NULL);
+	}
+
+	rfb_send_screen(rc, cfd, 1);
+
+	pthread_create(&tid, NULL, rfb_wr_thr, rc);
+	pthread_set_name_np(tid, "rfbout");
+
+        /* Now read in client requests. 1st byte identifies type */
+	for (;;) {
+		len = read(cfd, buf, 1);
+		if (len <= 0) {
+			DPRINTF(("rfb client exiting\r\n"));
+			break;
+		}
+
+		switch (buf[0]) {
+		case 0:
+			rfb_recv_set_pixfmt_msg(rc, cfd);
+			break;
+		case 2:
+			rfb_recv_set_encodings_msg(rc, cfd);
+			break;
+		case 3:
+			rfb_recv_update_msg(rc, cfd, 1);
+			break;
+		case 4:
+			rfb_recv_key_msg(rc, cfd);
+			break;
+		case 5:
+			rfb_recv_ptr_msg(rc, cfd);
+			break;
+		case 6:
+			rfb_recv_cuttext_msg(rc, cfd);
+			break;
+		default:
+			WPRINTF(("rfb unknown cli-code %d!\n", buf[0] & 0xff));
+			goto done;
+		}
+	}
+done:
+	rc->cfd = -1;
+	pthread_join(tid, NULL);
+	if (rc->enc_zlib_ok)
+		deflateEnd(&rc->zstream);
+}
+
+static void *
+rfb_thr(void *arg)
+{
+	struct rfb_softc *rc;
+	sigset_t set;
+
+	int cfd;
+
+	rc = arg;
+
+	sigemptyset(&set);
+	sigaddset(&set, SIGPIPE);
+	if (pthread_sigmask(SIG_BLOCK, &set, NULL) != 0) {
+		perror("pthread_sigmask");
+		return (NULL);
+	}
+
+	for (;;) {
+		rc->enc_raw_ok = false;
+		rc->enc_zlib_ok = false;
+		rc->enc_resize_ok = false;
+
+		cfd = accept(rc->sfd, NULL, NULL);
+		if (rc->conn_wait) {
+			pthread_mutex_lock(&rc->mtx);
+			pthread_cond_signal(&rc->cond);
+			pthread_mutex_unlock(&rc->mtx);
+			rc->conn_wait = 0;
+		}
+		rfb_handle(rc, cfd);
+		close(cfd);
+	}
+
+	/* NOTREACHED */
+	return (NULL);
+}
+
+static int
+sse42_supported()
+{
+	unsigned int eax, ebx, ecx, edx;
+ 
+	__get_cpuid(1, &eax, &ebx, &ecx, &edx);
+ 
+	return ((ecx & bit_SSE42) != 0);
+}
+
+int
+rfb_init(char *hostname, int port, int wait)
+{
+	struct rfb_softc *rc;
+	struct sockaddr_in sin;
+	int on = 1;
+
+	rc = calloc(1, sizeof(struct rfb_softc));
+
+	rc->crc = calloc(howmany(RFB_MAX_WIDTH * RFB_MAX_HEIGHT, 32),
+	                 sizeof(uint32_t));
+	rc->crc_tmp = calloc(howmany(RFB_MAX_WIDTH * RFB_MAX_HEIGHT, 32),
+	                     sizeof(uint32_t));
+	rc->crc_width = RFB_MAX_WIDTH;
+	rc->crc_height = RFB_MAX_HEIGHT;
+
+	rc->sfd = socket(AF_INET, SOCK_STREAM, 0);
+	if (rc->sfd < 0) {
+		perror("socket");
+		return (-1);
+	}
+
+	setsockopt(rc->sfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
+
+	sin.sin_len = sizeof(sin);
+	sin.sin_family = AF_INET;
+	sin.sin_port = htons(port);
+	if (hostname && strlen(hostname) > 0)
+		inet_pton(AF_INET, hostname, &(sin.sin_addr));
+	else
+		sin.sin_addr.s_addr = htonl(INADDR_ANY);
+
+	if (bind(rc->sfd, (struct sockaddr *)&sin, sizeof(sin)) < 0) {
+		perror("bind");
+		return (-1);
+	}
+
+	if (listen(rc->sfd, 1) < 0) {
+		perror("listen");
+		return (-1);
+	}
+
+	rc->hw_crc = sse42_supported();
+
+	rc->conn_wait = wait;
+	if (wait) {
+		pthread_mutex_init(&rc->mtx, NULL);
+		pthread_cond_init(&rc->cond, NULL);
+	}
+
+	pthread_create(&rc->tid, NULL, rfb_thr, rc);
+	pthread_set_name_np(rc->tid, "rfb");
+
+	if (wait) {
+		DPRINTF(("Waiting for rfb client...\n"));
+		pthread_mutex_lock(&rc->mtx);
+		pthread_cond_wait(&rc->cond, &rc->mtx);
+		pthread_mutex_unlock(&rc->mtx);
+	}
+
+	return (0);
+}
diff --git a/rfb.h b/rfb.h
new file mode 100644
index 000000000000..b3d3786669cc
--- /dev/null
+++ b/rfb.h
@@ -0,0 +1,36 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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$
+ */
+
+#ifndef _RFB_H_
+#define	_RFB_H_
+
+#define	RFB_PORT	5900
+
+int	rfb_init(char *hostname, int port, int wait);
+
+#endif /* _RFB_H_ */
diff --git a/sockstream.c b/sockstream.c
new file mode 100644
index 000000000000..1789206ff398
--- /dev/null
+++ b/sockstream.c
@@ -0,0 +1,86 @@
+/*-
+ * Copyright (c) 2015 Nahanni Systems, Inc.
+ * 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 ``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$
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <errno.h>
+
+#include "sockstream.h"
+
+ssize_t
+stream_read(int fd, void *buf, ssize_t nbytes)
+{
+	uint8_t *p;
+	ssize_t len = 0;
+	ssize_t n;
+
+	p = buf;
+
+	while (len < nbytes) {
+		n = read(fd, p + len, nbytes - len);
+		if (n == 0)
+			break;
+
+		if (n < 0) {
+			if (errno == EINTR || errno == EAGAIN)
+				continue;
+			return (n);
+		}
+		len += n;
+	}
+	return (len);
+}
+
+ssize_t
+stream_write(int fd, const void *buf, ssize_t nbytes)
+{
+	const uint8_t *p;
+	ssize_t len = 0;
+	ssize_t n;
+
+	p = buf;
+
+	while (len < nbytes) {
+		n = write(fd, p + len, nbytes - len);
+		if (n == 0)
+			break;
+		if (n < 0) {
+			if (errno == EINTR || errno == EAGAIN)
+				continue;
+			return (n);
+		}
+		len += n;
+	}
+	return (len);
+}
+
+
diff --git a/sockstream.h b/sockstream.h
new file mode 100644
index 000000000000..bb0b3b06eb9c
--- /dev/null
+++ b/sockstream.h
@@ -0,0 +1,33 @@
+/*-
+ * Copyright (c) 2015 Nahanni Systems, Inc.
+ * 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 ``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$
+ */
+
+#include <sys/types.h>
+#include <unistd.h>
+
+ssize_t stream_read(int fd, void *buf, ssize_t nbytes);
+ssize_t stream_write(int fd, const void *buf, ssize_t nbytes);
diff --git a/usb_emul.c b/usb_emul.c
new file mode 100644
index 000000000000..3dc12a5c3c1b
--- /dev/null
+++ b/usb_emul.c
@@ -0,0 +1,76 @@
+/*-
+ * Copyright (c) 2014 Nahanni Systems Inc.
+ * 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>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+#include <sys/queue.h>
+
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <pthread.h>
+
+#include "usb_emul.h"
+
+SET_DECLARE(usb_emu_set, struct usb_devemu);
+
+struct usb_devemu *
+usb_emu_finddev(char *name)
+{
+	struct usb_devemu **udpp, *udp;
+
+	SET_FOREACH(udpp, usb_emu_set) {
+		udp = *udpp;
+		if (!strcmp(udp->ue_emu, name))
+			return (udp);
+	}
+
+	return (NULL);
+}
+
+struct usb_data_xfer_block *
+usb_data_xfer_append(struct usb_data_xfer *xfer, void *buf, int blen,
+                     void *hci_data, int ccs)
+{
+	struct usb_data_xfer_block *xb;
+
+	if (xfer->ndata >= USB_MAX_XFER_BLOCKS)
+		return (NULL);
+
+	xb = &xfer->data[xfer->tail];
+	xb->buf = buf;
+	xb->blen = blen;
+	xb->hci_data = hci_data;
+	xb->ccs = ccs;
+	xb->processed = 0;
+	xb->bdone = 0;
+	xfer->ndata++;
+	xfer->tail = (xfer->tail + 1) % USB_MAX_XFER_BLOCKS;
+	return (xb);
+}
diff --git a/usb_emul.h b/usb_emul.h
new file mode 100644
index 000000000000..69df135466b5
--- /dev/null
+++ b/usb_emul.h
@@ -0,0 +1,156 @@
+/*-
+ * Copyright (c) 2014 Leon Dang <ldang@nahannisys.com>
+ * 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$
+ */
+
+#ifndef _USB_EMUL_H_
+#define _USB_EMUL_H_
+
+#include <stdlib.h>
+#include <sys/linker_set.h>
+#include <pthread.h>
+
+#define	USB_MAX_XFER_BLOCKS	8
+
+#define	USB_XFER_OUT		0
+#define	USB_XFER_IN		1
+
+
+
+struct usb_hci;
+struct usb_device_request;
+struct usb_data_xfer;
+
+/* Device emulation handlers */
+struct usb_devemu {
+	char	*ue_emu;	/* name of device emulation */
+	int	ue_usbver;	/* usb version: 2 or 3 */
+	int	ue_usbspeed;	/* usb device speed */
+
+	/* instance creation */
+	void	*(*ue_init)(struct usb_hci *hci, char *opt);
+
+	/* handlers */
+	int	(*ue_request)(void *sc, struct usb_data_xfer *xfer);
+	int	(*ue_data)(void *sc, struct usb_data_xfer *xfer, int dir,
+	                   int epctx);
+	int	(*ue_reset)(void *sc);
+	int	(*ue_remove)(void *sc);
+	int	(*ue_stop)(void *sc);
+};
+#define	USB_EMUL_SET(x)		DATA_SET(usb_emu_set, x);
+
+/*
+ * USB device events to notify HCI when state changes
+ */
+enum hci_usbev {
+	USBDEV_ATTACH,
+	USBDEV_RESET,
+	USBDEV_STOP,
+	USBDEV_REMOVE,
+};
+
+/* usb controller, ie xhci, ehci */
+struct usb_hci {
+	int	(*hci_intr)(struct usb_hci *hci, int epctx);
+	int	(*hci_event)(struct usb_hci *hci, enum hci_usbev evid,
+		             void *param);
+	void	*hci_sc;			/* private softc for hci */
+
+	/* controller managed fields */
+	int	hci_address;
+	int	hci_port;
+};
+
+/*
+ * Each xfer block is mapped to the hci transfer block.
+ * On input into the device handler, blen is set to the lenght of buf.
+ * The device handler is to update blen to reflect on the residual size
+ * of the buffer, i.e. len(buf) - len(consumed).
+ */
+struct usb_data_xfer_block {
+	void	*buf;			/* IN or OUT pointer */
+	int	blen;			/* in:len(buf), out:len(remaining) */
+	int	bdone;			/* bytes transferred */
+	uint32_t processed;		/* device processed this + errcode */
+	void	*hci_data;		/* HCI private reference */
+	int	ccs;
+	uint32_t streamid;
+	uint64_t trbnext;		/* next TRB guest address */
+};
+
+struct usb_data_xfer {
+	struct usb_data_xfer_block data[USB_MAX_XFER_BLOCKS];
+	struct usb_device_request *ureq; 	/* setup ctl request */
+	int	ndata;				/* # of data items */
+	int	head;
+	int	tail;
+	pthread_mutex_t mtx;
+};
+
+enum USB_ERRCODE {
+	USB_ACK,
+	USB_NAK,
+	USB_STALL,
+	USB_NYET,
+	USB_ERR,
+	USB_SHORT
+};
+
+#define	USB_DATA_GET_ERRCODE(x)		(x)->processed >> 8
+#define	USB_DATA_SET_ERRCODE(x,e)	do {				\
+			(x)->processed = ((x)->processed & 0xFF) | (e << 8); \
+		} while (0)
+
+#define	USB_DATA_OK(x,i)	((x)->data[(i)].buf != NULL)
+
+#define	USB_DATA_XFER_INIT(x)	do {					\
+			memset((x), 0, sizeof(*(x)));			\
+			pthread_mutex_init(&((x)->mtx), NULL);		\
+		} while (0)
+
+#define	USB_DATA_XFER_RESET(x)	do {					\
+			memset((x)->data, 0, sizeof((x)->data));	\
+			(x)->ndata = 0;					\
+			(x)->head = (x)->tail = 0;			\
+		} while (0)
+
+#define	USB_DATA_XFER_LOCK(x)	do {					\
+			pthread_mutex_lock(&((x)->mtx));		\
+		} while (0)
+
+#define	USB_DATA_XFER_UNLOCK(x)	do {					\
+			pthread_mutex_unlock(&((x)->mtx));		\
+		} while (0)
+
+
+struct usb_devemu *usb_emu_finddev(char *name);
+
+struct usb_data_xfer_block *usb_data_xfer_append(struct usb_data_xfer *xfer,
+                          void *buf, int blen, void *hci_data, int ccs);
+
+
+#endif /* _USB_EMUL_H_ */
diff --git a/usb_mouse.c b/usb_mouse.c
new file mode 100644
index 000000000000..c2f21eb4feb6
--- /dev/null
+++ b/usb_mouse.c
@@ -0,0 +1,803 @@
+/*-
+ * Copyright (c) 2014 Leon Dang <ldang@nahannisys.com>
+ * 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>
+__FBSDID("$FreeBSD$");
+
+#include <sys/time.h>
+
+#include <pthread.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <dev/usb/usb.h>
+#include <dev/usb/usbdi.h>
+
+#include "usb_emul.h"
+#include "console.h"
+#include "bhyvegc.h"
+
+static int umouse_debug = 0;
+#define	DPRINTF(params) if (umouse_debug) printf params
+#define	WPRINTF(params) printf params
+
+/* USB endpoint context (1-15) for reporting mouse data events*/
+#define	UMOUSE_INTR_ENDPT	1
+
+#define UMOUSE_REPORT_DESC_TYPE	0x22
+
+#define	UMOUSE_GET_REPORT	0x01
+#define	UMOUSE_GET_IDLE		0x02
+#define	UMOUSE_GET_PROTOCOL	0x03
+#define	UMOUSE_SET_REPORT	0x09
+#define	UMOUSE_SET_IDLE		0x0A
+#define	UMOUSE_SET_PROTOCOL	0x0B
+
+#define HSETW(ptr, val)   ptr = { (uint8_t)(val), (uint8_t)((val) >> 8) }
+
+enum {
+	UMSTR_LANG,
+	UMSTR_MANUFACTURER,
+	UMSTR_PRODUCT,
+	UMSTR_SERIAL,
+	UMSTR_CONFIG,
+	UMSTR_MAX
+};
+
+static const char *umouse_desc_strings[] = {
+	"\x04\x09",
+	"BHYVE",
+	"HID Tablet",
+	"01",
+	"HID Tablet Device",
+};
+
+struct umouse_hid_descriptor {
+	uint8_t	bLength;
+	uint8_t	bDescriptorType;
+	uint8_t	bcdHID[2];
+	uint8_t	bCountryCode;
+	uint8_t	bNumDescriptors;
+	uint8_t	bReportDescriptorType;
+	uint8_t	wItemLength[2];
+} __packed;
+
+struct umouse_config_desc {
+	struct usb_config_descriptor		confd;
+	struct usb_interface_descriptor		ifcd;
+	struct umouse_hid_descriptor		hidd;
+	struct usb_endpoint_descriptor		endpd;
+	struct usb_endpoint_ss_comp_descriptor	sscompd;
+} __packed;
+
+#define MOUSE_MAX_X	0x8000
+#define MOUSE_MAX_Y	0x8000
+
+static const uint8_t umouse_report_desc[] = {
+	0x05, 0x01,		/* USAGE_PAGE (Generic Desktop)		*/
+	0x09, 0x02,		/* USAGE (Mouse)			*/
+	0xa1, 0x01,		/* COLLECTION (Application) 		*/
+	0x09, 0x01,		/*   USAGE (Pointer)			*/
+	0xa1, 0x00,		/*   COLLECTION (Physical)		*/
+	0x05, 0x09,		/*     USAGE_PAGE (Button)		*/
+	0x19, 0x01,		/*     USAGE_MINIMUM (Button 1)		*/
+	0x29, 0x03,		/*     USAGE_MAXIMUM (Button 3)		*/
+	0x15, 0x00,		/*     LOGICAL_MINIMUM (0)		*/
+	0x25, 0x01,		/*     LOGICAL_MAXIMUM (1)		*/
+	0x75, 0x01,		/*     REPORT_SIZE (1)			*/
+	0x95, 0x03,		/*     REPORT_COUNT (3)			*/
+	0x81, 0x02,		/*     INPUT (Data,Var,Abs); 3 buttons	*/
+	0x75, 0x05,		/*     REPORT_SIZE (5)			*/
+	0x95, 0x01,		/*     REPORT_COUNT (1)			*/
+	0x81, 0x03,		/*     INPUT (Cnst,Var,Abs); padding	*/
+	0x05, 0x01,		/*     USAGE_PAGE (Generic Desktop)	*/
+	0x09, 0x30,		/*     USAGE (X)			*/
+	0x09, 0x31,		/*     USAGE (Y)			*/
+	0x35, 0x00,		/*     PHYSICAL_MINIMUM (0)		*/
+	0x46, 0xff, 0x7f,	/*     PHYSICAL_MAXIMUM (0x7fff)	*/
+	0x15, 0x00,		/*     LOGICAL_MINIMUM (0)		*/
+	0x26, 0xff, 0x7f,	/*     LOGICAL_MAXIMUM (0x7fff)		*/
+	0x75, 0x10,		/*     REPORT_SIZE (16)			*/
+	0x95, 0x02,		/*     REPORT_COUNT (2)			*/
+	0x81, 0x02,		/*     INPUT (Data,Var,Abs)		*/
+	0x05, 0x01,		/*     USAGE Page (Generic Desktop)	*/
+	0x09, 0x38,		/*     USAGE (Wheel)			*/
+	0x35, 0x00,		/*     PHYSICAL_MINIMUM (0)		*/
+	0x45, 0x00,		/*     PHYSICAL_MAXIMUM (0)		*/
+	0x15, 0x81,		/*     LOGICAL_MINIMUM (-127)		*/
+	0x25, 0x7f,		/*     LOGICAL_MAXIMUM (127)		*/
+	0x75, 0x08,		/*     REPORT_SIZE (8)			*/
+	0x95, 0x01,		/*     REPORT_COUNT (1)			*/
+	0x81, 0x06,		/*     INPUT (Data,Var,Rel)		*/
+	0xc0,			/*   END_COLLECTION			*/
+	0xc0			/* END_COLLECTION			*/
+};
+
+struct umouse_report {
+	uint8_t	buttons;	/* bits: 0 left, 1 right, 2 middle */
+	int16_t	x;		/* x position */
+	int16_t	y;		/* y position */
+	int8_t	z;		/* z wheel position */
+} __packed;
+
+
+#define	MSETW(ptr, val)	ptr = { (uint8_t)(val), (uint8_t)((val) >> 8) }
+
+static struct usb_device_descriptor umouse_dev_desc = {
+	.bLength = sizeof(umouse_dev_desc),
+	.bDescriptorType = UDESC_DEVICE,
+	MSETW(.bcdUSB, UD_USB_3_0),
+	.bMaxPacketSize = 8,			/* max packet size */
+	MSETW(.idVendor, 0xFB5D),		/* vendor */
+	MSETW(.idProduct, 0x0001),		/* product */
+	MSETW(.bcdDevice, 0),			/* device version */
+	.iManufacturer = UMSTR_MANUFACTURER,
+	.iProduct = UMSTR_PRODUCT,
+	.iSerialNumber = UMSTR_SERIAL,
+	.bNumConfigurations = 1,
+};
+
+static struct umouse_config_desc umouse_confd = {
+	.confd = {
+		.bLength = sizeof(umouse_confd.confd),
+		.bDescriptorType = UDESC_CONFIG,
+		.wTotalLength[0] = sizeof(umouse_confd),
+		.bNumInterface = 1,
+		.bConfigurationValue = 1,
+		.iConfiguration = UMSTR_CONFIG,
+		.bmAttributes = UC_BUS_POWERED | UC_REMOTE_WAKEUP,
+		.bMaxPower = 0,
+	},
+	.ifcd = {
+		.bLength = sizeof(umouse_confd.ifcd),
+		.bDescriptorType = UDESC_INTERFACE,
+		.bNumEndpoints = 1,
+		.bInterfaceClass = UICLASS_HID,
+		.bInterfaceSubClass = UISUBCLASS_BOOT,
+		.bInterfaceProtocol = UIPROTO_MOUSE,
+	},
+	.hidd = {
+		.bLength = sizeof(umouse_confd.hidd),
+		.bDescriptorType = 0x21,
+		.bcdHID = { 0x01, 0x10 },
+		.bCountryCode = 0,
+		.bNumDescriptors = 1,
+		.bReportDescriptorType = UMOUSE_REPORT_DESC_TYPE,
+		.wItemLength = { sizeof(umouse_report_desc), 0 },
+	},
+	.endpd = {
+		.bLength = sizeof(umouse_confd.endpd),
+		.bDescriptorType = UDESC_ENDPOINT,
+		.bEndpointAddress = UE_DIR_IN | UMOUSE_INTR_ENDPT,
+		.bmAttributes = UE_INTERRUPT,
+		.wMaxPacketSize[0] = 8,
+		.bInterval = 0xA,
+	},
+	.sscompd = {
+		.bLength = sizeof(umouse_confd.sscompd),
+		.bDescriptorType = UDESC_ENDPOINT_SS_COMP,
+		.bMaxBurst = 0,
+		.bmAttributes = 0,
+		MSETW(.wBytesPerInterval, 0),
+	},
+};
+
+
+struct umouse_bos_desc {
+	struct usb_bos_descriptor		bosd;
+	struct usb_devcap_ss_descriptor		usbssd;
+} __packed;
+
+
+struct umouse_bos_desc umouse_bosd = {
+	.bosd = {
+		.bLength = sizeof(umouse_bosd.bosd),
+		.bDescriptorType = UDESC_BOS,
+		HSETW(.wTotalLength, sizeof(umouse_bosd)),
+		.bNumDeviceCaps = 1,
+	},
+        .usbssd = {
+                .bLength = sizeof(umouse_bosd.usbssd),
+                .bDescriptorType = UDESC_DEVICE_CAPABILITY,
+                .bDevCapabilityType = 3,
+                .bmAttributes = 0,
+                HSETW(.wSpeedsSupported, 0x08),
+                .bFunctionalitySupport = 3,
+                .bU1DevExitLat = 0xa,   /* dummy - not used */
+                .wU2DevExitLat = { 0x20, 0x00 },
+        }
+};
+
+
+struct umouse_softc {
+	struct usb_hci *hci;
+
+	char	*opt;
+
+	struct umouse_report um_report;
+	int	newdata;
+	struct {
+		uint8_t	idle;
+		uint8_t	protocol;
+		uint8_t	feature;
+	} hid;
+
+	pthread_mutex_t	mtx;
+	pthread_mutex_t	ev_mtx;
+	int		polling;
+	struct timeval	prev_evt;
+};
+
+static void
+umouse_event(uint8_t button, int x, int y, void *arg)
+{
+	struct umouse_softc *sc;
+	struct bhyvegc_image *gc;
+
+	gc = console_get_image();
+	if (gc == NULL) {
+		/* not ready */
+		return;
+	}
+
+	sc = arg;
+
+	pthread_mutex_lock(&sc->mtx);
+
+	sc->um_report.buttons = 0;
+	sc->um_report.z = 0;
+
+	if (button & 0x01)
+		sc->um_report.buttons |= 0x01;	/* left */
+	if (button & 0x02)
+		sc->um_report.buttons |= 0x04;	/* middle */
+	if (button & 0x04)
+		sc->um_report.buttons |= 0x02;	/* right */
+	if (button & 0x8)
+		sc->um_report.z = 1;
+	if (button & 0x10)
+		sc->um_report.z = -1;
+
+	/* scale coords to mouse resolution */
+	sc->um_report.x = MOUSE_MAX_X * x / gc->width;
+	sc->um_report.y = MOUSE_MAX_X * y / gc->height;
+	sc->newdata = 1;
+	pthread_mutex_unlock(&sc->mtx);
+
+	pthread_mutex_lock(&sc->ev_mtx);
+	sc->hci->hci_intr(sc->hci, UE_DIR_IN | UMOUSE_INTR_ENDPT);
+	pthread_mutex_unlock(&sc->ev_mtx);
+}
+
+static void *
+umouse_init(struct usb_hci *hci, char *opt)
+{
+	struct umouse_softc *sc;
+	char *mopt;
+
+	mopt = opt;
+
+	sc = calloc(1, sizeof(struct umouse_softc));
+	sc->hci = hci;
+
+	sc->hid.protocol = 1;	/* REPORT protocol */
+	sc->opt = strdup(opt);
+	pthread_mutex_init(&sc->mtx, NULL);
+	pthread_mutex_init(&sc->ev_mtx, NULL);
+
+	console_ptr_register(umouse_event, sc, 10);
+
+	return (sc);
+}
+
+#define	UREQ(x,y)	((x) | ((y) << 8))
+
+static int
+umouse_request(void *scarg, struct usb_data_xfer *xfer)
+{
+	struct umouse_softc *sc;
+	struct usb_data_xfer_block *data;
+	const char *str;
+	uint16_t value;
+	uint16_t index;
+	uint16_t len;
+	uint16_t slen;
+	uint8_t *udata;
+	int	err;
+	int	i, idx;
+	int	eshort;
+
+	sc = scarg;
+
+	data = NULL;
+	udata = NULL;
+	idx = xfer->head;
+	for (i = 0; i < xfer->ndata; i++) {
+		xfer->data[idx].bdone = 0;
+		if (data == NULL && USB_DATA_OK(xfer,i)) {
+			data = &xfer->data[idx];
+			udata = data->buf;
+		}
+
+		xfer->data[idx].processed = 1;
+		idx = (idx + 1) % USB_MAX_XFER_BLOCKS;
+	}
+
+	err = USB_ERR_NORMAL_COMPLETION;
+	eshort = 0;
+
+	if (!xfer->ureq) {
+		DPRINTF(("umouse_request: port %d\r\n", sc->hci->hci_port));
+		goto done;
+	}
+
+	value = UGETW(xfer->ureq->wValue);
+	index = UGETW(xfer->ureq->wIndex);
+	len = UGETW(xfer->ureq->wLength);
+
+	DPRINTF(("umouse_request: port %d, type 0x%x, req 0x%x, val 0x%x, "
+	         "idx 0x%x, len %u\r\n",
+	         sc->hci->hci_port, xfer->ureq->bmRequestType,
+	         xfer->ureq->bRequest, value, index, len));
+
+	switch (UREQ(xfer->ureq->bRequest, xfer->ureq->bmRequestType)) {
+	case UREQ(UR_GET_CONFIG, UT_READ_DEVICE):
+		DPRINTF(("umouse: (UR_GET_CONFIG, UT_READ_DEVICE)\r\n"));
+		if (!data)
+			break;
+
+		*udata = umouse_confd.confd.bConfigurationValue;
+		data->blen = len > 0 ? len - 1 : 0;
+		eshort = data->blen > 0;
+		data->bdone += 1;
+		break;
+
+	case UREQ(UR_GET_DESCRIPTOR, UT_READ_DEVICE):
+		DPRINTF(("umouse: (UR_GET_DESCRIPTOR, UT_READ_DEVICE) val %x\r\n",
+		        value >> 8));
+		if (!data)
+			break;
+
+		switch (value >> 8) {
+		case UDESC_DEVICE:
+			DPRINTF(("umouse: (->UDESC_DEVICE) len %u ?= "
+			         "sizeof(umouse_dev_desc) %lu\r\n",
+			         len, sizeof(umouse_dev_desc)));
+			if ((value & 0xFF) != 0) {
+				err = USB_ERR_IOERROR;
+				goto done;
+			}
+			if (len > sizeof(umouse_dev_desc)) {
+				data->blen = len - sizeof(umouse_dev_desc);
+				len = sizeof(umouse_dev_desc);
+			} else
+				data->blen = 0;
+			memcpy(data->buf, &umouse_dev_desc, len);
+			data->bdone += len;
+			break;
+
+		case UDESC_CONFIG:
+			DPRINTF(("umouse: (->UDESC_CONFIG)\r\n"));
+			if ((value & 0xFF) != 0) {
+				err = USB_ERR_IOERROR;
+				goto done;
+			}
+			if (len > sizeof(umouse_confd)) {
+				data->blen = len - sizeof(umouse_confd);
+				len = sizeof(umouse_confd);
+			} else
+				data->blen = 0;
+
+			memcpy(data->buf, &umouse_confd, len);
+			data->bdone += len;
+			break;
+
+		case UDESC_STRING:
+			DPRINTF(("umouse: (->UDESC_STRING)\r\n"));
+			str = NULL;
+			if ((value & 0xFF) < UMSTR_MAX)
+				str = umouse_desc_strings[value & 0xFF];
+			else
+				goto done;
+
+			if ((value & 0xFF) == UMSTR_LANG) {
+				udata[0] = 4;
+				udata[1] = UDESC_STRING;
+				data->blen = len - 2;
+				len -= 2;
+				data->bdone += 2;
+
+				if (len >= 2) {
+					udata[2] = str[0];
+					udata[3] = str[1];
+					data->blen -= 2;
+					data->bdone += 2;
+				} else
+					data->blen = 0;
+
+				goto done;
+			}
+
+			slen = 2 + strlen(str) * 2;
+			udata[0] = slen;
+			udata[1] = UDESC_STRING;
+
+			if (len > slen) {
+				data->blen = len - slen;
+				len = slen;
+			} else
+				data->blen = 0;
+			for (i = 2; i < len; i += 2) {
+				udata[i] = *str++;
+				udata[i+1] = '\0';
+			}
+			data->bdone += slen;
+
+			break;
+
+		case UDESC_BOS:
+			DPRINTF(("umouse: USB3 BOS\r\n"));
+			if (len > sizeof(umouse_bosd)) {
+				data->blen = len - sizeof(umouse_bosd);
+				len = sizeof(umouse_bosd);
+			} else
+				data->blen = 0;
+			memcpy(udata, &umouse_bosd, len);
+			data->bdone += len;
+			break;
+
+		default:
+			DPRINTF(("umouse: unknown(%d)->ERROR\r\n", value >> 8));
+			err = USB_ERR_IOERROR;
+			goto done;
+		}
+		eshort = data->blen > 0;
+		break;
+
+	case UREQ(UR_GET_DESCRIPTOR, UT_READ_INTERFACE):
+		DPRINTF(("umouse: (UR_GET_DESCRIPTOR, UT_READ_INTERFACE) "
+		         "0x%x\r\n", (value >> 8)));
+		if (!data)
+			break;
+
+		switch (value >> 8) {
+		case UMOUSE_REPORT_DESC_TYPE:
+			if (len > sizeof(umouse_report_desc)) {
+				data->blen = len - sizeof(umouse_report_desc);
+				len = sizeof(umouse_report_desc);
+			} else
+				data->blen = 0;
+			memcpy(data->buf, umouse_report_desc, len);
+			data->bdone += len;
+			break;
+		default:
+			DPRINTF(("umouse: IO ERROR\r\n"));
+			err = USB_ERR_IOERROR;
+			goto done;
+		}
+		eshort = data->blen > 0;
+		break;
+
+	case UREQ(UR_GET_INTERFACE, UT_READ_INTERFACE):
+		DPRINTF(("umouse: (UR_GET_INTERFACE, UT_READ_INTERFACE)\r\n"));
+		if (index != 0) {
+			DPRINTF(("umouse get_interface, invalid index %d\r\n",
+			        index));
+			err = USB_ERR_IOERROR;
+			goto done;
+		}
+
+		if (!data)
+			break;
+
+		if (len > 0) {
+			*udata = 0;
+			data->blen = len - 1;
+		}
+		eshort = data->blen > 0;
+		data->bdone += 1;
+		break;
+
+	case UREQ(UR_GET_STATUS, UT_READ_DEVICE):
+		DPRINTF(("umouse: (UR_GET_STATUS, UT_READ_DEVICE)\r\n"));
+		if (data != NULL && len > 1) {
+			if (sc->hid.feature == UF_DEVICE_REMOTE_WAKEUP)
+				USETW(udata, UDS_REMOTE_WAKEUP);
+			else
+				USETW(udata, 0);
+			data->blen = len - 2;
+			data->bdone += 2;
+		}
+
+		eshort = data->blen > 0;
+		break;
+
+	case UREQ(UR_GET_STATUS, UT_READ_INTERFACE): 
+	case UREQ(UR_GET_STATUS, UT_READ_ENDPOINT): 
+		DPRINTF(("umouse: (UR_GET_STATUS, UT_READ_INTERFACE)\r\n"));
+		if (data != NULL && len > 1) {
+			USETW(udata, 0);
+			data->blen = len - 2;
+			data->bdone += 2;
+		}
+		eshort = data->blen > 0;
+		break;
+
+	case UREQ(UR_SET_ADDRESS, UT_WRITE_DEVICE):
+		/* XXX Controller should've handled this */
+		DPRINTF(("umouse set address %u\r\n", value));
+		break;
+
+	case UREQ(UR_SET_CONFIG, UT_WRITE_DEVICE):
+		DPRINTF(("umouse set config %u\r\n", value));
+		break;
+
+	case UREQ(UR_SET_DESCRIPTOR, UT_WRITE_DEVICE):
+		DPRINTF(("umouse set descriptor %u\r\n", value));
+		break;
+
+
+	case UREQ(UR_CLEAR_FEATURE, UT_WRITE_DEVICE):
+		DPRINTF(("umouse: (UR_SET_FEATURE, UT_WRITE_DEVICE) %x\r\n", value));
+		if (value == UF_DEVICE_REMOTE_WAKEUP)
+			sc->hid.feature = 0;
+		break;
+
+	case UREQ(UR_SET_FEATURE, UT_WRITE_DEVICE):
+		DPRINTF(("umouse: (UR_SET_FEATURE, UT_WRITE_DEVICE) %x\r\n", value));
+		if (value == UF_DEVICE_REMOTE_WAKEUP)
+			sc->hid.feature = UF_DEVICE_REMOTE_WAKEUP;
+		break;
+
+	case UREQ(UR_CLEAR_FEATURE, UT_WRITE_INTERFACE):
+	case UREQ(UR_CLEAR_FEATURE, UT_WRITE_ENDPOINT):
+	case UREQ(UR_SET_FEATURE, UT_WRITE_INTERFACE):
+	case UREQ(UR_SET_FEATURE, UT_WRITE_ENDPOINT):
+		DPRINTF(("umouse: (UR_CLEAR_FEATURE, UT_WRITE_INTERFACE)\r\n"));
+		err = USB_ERR_IOERROR;
+		goto done;
+
+	case UREQ(UR_SET_INTERFACE, UT_WRITE_INTERFACE):
+		DPRINTF(("umouse set interface %u\r\n", value));
+		break;
+
+	case UREQ(UR_ISOCH_DELAY, UT_WRITE_DEVICE):
+		DPRINTF(("umouse set isoch delay %u\r\n", value));
+		break;
+
+	case UREQ(UR_SET_SEL, 0):
+		DPRINTF(("umouse set sel\r\n"));
+		break;
+
+	case UREQ(UR_SYNCH_FRAME, UT_WRITE_ENDPOINT):
+		DPRINTF(("umouse synch frame\r\n"));
+		break;
+
+	/* HID device requests */
+
+	case UREQ(UMOUSE_GET_REPORT, UT_READ_CLASS_INTERFACE):
+		DPRINTF(("umouse: (UMOUSE_GET_REPORT, UT_READ_CLASS_INTERFACE) "
+		         "0x%x\r\n", (value >> 8)));
+		if (!data)
+			break;
+
+		if ((value >> 8) == 0x01 && len >= sizeof(sc->um_report)) {
+			/* TODO read from backend */
+
+			if (len > sizeof(sc->um_report)) {
+				data->blen = len - sizeof(sc->um_report);
+				len = sizeof(sc->um_report);
+			} else
+				data->blen = 0;
+
+			memcpy(data->buf, &sc->um_report, len);
+			data->bdone += len;
+		} else {
+			err = USB_ERR_IOERROR;
+			goto done;
+		}
+		eshort = data->blen > 0;
+		break;
+
+	case UREQ(UMOUSE_GET_IDLE, UT_READ_CLASS_INTERFACE):
+		if (data != NULL && len > 0) {
+			*udata = sc->hid.idle;
+			data->blen = len - 1;
+			data->bdone += 1;
+		}
+		eshort = data->blen > 0;
+		break;
+
+	case UREQ(UMOUSE_GET_PROTOCOL, UT_READ_CLASS_INTERFACE):
+		if (data != NULL && len > 0) {
+			*udata = sc->hid.protocol;
+			data->blen = len - 1;
+			data->bdone += 1;
+		}
+		eshort = data->blen > 0;
+		break;
+
+	case UREQ(UMOUSE_SET_REPORT, UT_WRITE_CLASS_INTERFACE):
+		DPRINTF(("umouse: (UMOUSE_SET_REPORT, UT_WRITE_CLASS_INTERFACE) ignored\r\n"));
+		break;
+
+	case UREQ(UMOUSE_SET_IDLE, UT_WRITE_CLASS_INTERFACE):
+		sc->hid.idle = UGETW(xfer->ureq->wValue) >> 8;
+		DPRINTF(("umouse: (UMOUSE_SET_IDLE, UT_WRITE_CLASS_INTERFACE) %x\r\n",
+		        sc->hid.idle));
+		break;
+
+	case UREQ(UMOUSE_SET_PROTOCOL, UT_WRITE_CLASS_INTERFACE):
+		sc->hid.protocol = UGETW(xfer->ureq->wValue) >> 8;
+		DPRINTF(("umouse: (UR_CLEAR_FEATURE, UT_WRITE_CLASS_INTERFACE) %x\r\n",
+		        sc->hid.protocol));
+		break;
+
+	default:
+		DPRINTF(("**** umouse request unhandled\r\n"));
+		err = USB_ERR_IOERROR;
+		break;
+	}
+
+done:
+	if (xfer->ureq && (xfer->ureq->bmRequestType & UT_WRITE) &&
+	    (err == USB_ERR_NORMAL_COMPLETION) && (data != NULL))
+		data->blen = 0;
+	else if (eshort)
+		err = USB_ERR_SHORT_XFER;
+
+	DPRINTF(("umouse request error code %d (0=ok), blen %u txlen %u\r\n",
+	        err, (data ? data->blen : 0), (data ? data->bdone : 0)));
+
+	return (err);
+}
+
+static int
+umouse_data_handler(void *scarg, struct usb_data_xfer *xfer, int dir,
+     int epctx)
+{
+	struct umouse_softc *sc;
+	struct usb_data_xfer_block *data;
+	uint8_t *udata;
+	int len, i, idx;
+	int err;
+
+	DPRINTF(("umouse handle data - DIR=%s|EP=%d, blen %d\r\n",
+	        dir ? "IN" : "OUT", epctx, xfer->data[0].blen));
+
+
+	/* find buffer to add data */
+	udata = NULL;
+	err = USB_ERR_NORMAL_COMPLETION;
+
+	/* handle xfer at first unprocessed item with buffer */
+	data = NULL;
+	idx = xfer->head;
+	for (i = 0; i < xfer->ndata; i++) {
+		data = &xfer->data[idx];
+		if (data->buf != NULL && data->blen != 0) {
+			break;
+		} else {
+			data->processed = 1;
+			data = NULL;
+		}
+		idx = (idx + 1) % USB_MAX_XFER_BLOCKS;
+	}
+	if (!data)
+		goto done;
+
+	udata = data->buf;
+	len = data->blen;
+
+	if (udata == NULL) {
+		DPRINTF(("umouse no buffer provided for input\r\n"));
+		err = USB_ERR_NOMEM;
+		goto done;
+	}
+
+	sc = scarg;
+
+	if (dir) {
+
+		pthread_mutex_lock(&sc->mtx);
+
+		if (!sc->newdata) {
+			err = USB_ERR_CANCELLED;
+			USB_DATA_SET_ERRCODE(&xfer->data[xfer->head], USB_NAK);
+			pthread_mutex_unlock(&sc->mtx);
+			goto done;
+		}
+
+		if (sc->polling) {
+			err = USB_ERR_STALLED;
+			USB_DATA_SET_ERRCODE(data, USB_STALL);
+			pthread_mutex_unlock(&sc->mtx);
+			goto done;
+		}
+		sc->polling = 1;
+
+		if (len > 0) {
+			sc->newdata = 0;
+
+			data->processed = 1;
+			data->bdone += 6;
+			memcpy(udata, &sc->um_report, 6);
+			data->blen = len - 6;
+			if (data->blen > 0)
+				err = USB_ERR_SHORT_XFER;
+		}
+
+		sc->polling = 0;
+		pthread_mutex_unlock(&sc->mtx);
+	} else { 
+		USB_DATA_SET_ERRCODE(data, USB_STALL);
+		err = USB_ERR_STALLED;
+	}
+
+done:
+	return (err);
+}
+
+static int
+umouse_reset(void *scarg)
+{
+	struct umouse_softc *sc;
+
+	sc = scarg;
+
+	sc->newdata = 0;
+
+	return (0);
+}
+
+static int
+umouse_remove(void *scarg)
+{
+
+	return (0);
+}
+
+static int
+umouse_stop(void *scarg)
+{
+
+	return (0);
+}
+
+
+struct usb_devemu ue_mouse = {
+	.ue_emu =	"tablet",
+	.ue_usbver =	3,
+	.ue_usbspeed =	USB_SPEED_HIGH,
+	.ue_init =	umouse_init,
+	.ue_request =	umouse_request,
+	.ue_data =	umouse_data_handler,
+	.ue_reset =	umouse_reset,
+	.ue_remove =	umouse_remove,
+	.ue_stop =	umouse_stop
+};
+USB_EMUL_SET(ue_mouse);
diff --git a/vga.c b/vga.c
new file mode 100644
index 000000000000..208064b3b2ff
--- /dev/null
+++ b/vga.c
@@ -0,0 +1,1331 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+
+#include <assert.h>
+#include <pthread.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <machine/vmm.h>
+
+#include "bhyvegc.h"
+#include "console.h"
+#include "inout.h"
+#include "mem.h"
+#include "vga.h"
+
+#define	KB	(1024UL)
+#define	MB	(1024 * 1024UL)
+
+struct vga_softc {
+	struct mem_range	mr;
+
+	struct bhyvegc		*gc;
+	int			gc_width;
+	int			gc_height;
+	struct bhyvegc_image	*gc_image;
+
+	uint8_t			*vga_ram;
+
+	/*
+	 * General registers
+	 */
+	uint8_t			vga_misc;
+	uint8_t			vga_sts1;
+
+	/*
+	 * Sequencer
+	 */
+	struct {
+		int		seq_index;
+		uint8_t		seq_reset;
+		uint8_t		seq_clock_mode;
+		int		seq_cm_dots;
+		uint8_t		seq_map_mask;
+		uint8_t		seq_cmap_sel;
+		int		seq_cmap_pri_off;
+		int		seq_cmap_sec_off;
+		uint8_t		seq_mm;
+	} vga_seq;
+
+	/*
+	 * CRT Controller
+	 */
+	struct {
+		int		crtc_index;
+		uint8_t		crtc_mode_ctrl;
+		uint8_t		crtc_horiz_total;
+		uint8_t		crtc_horiz_disp_end;
+		uint8_t		crtc_start_horiz_blank;
+		uint8_t		crtc_end_horiz_blank;
+		uint8_t		crtc_start_horiz_retrace;
+		uint8_t		crtc_end_horiz_retrace;
+		uint8_t		crtc_vert_total;
+		uint8_t		crtc_overflow;
+		uint8_t		crtc_present_row_scan;
+		uint8_t		crtc_max_scan_line;
+		uint8_t		crtc_cursor_start;
+		uint8_t		crtc_cursor_on;
+		uint8_t		crtc_cursor_end;
+		uint8_t		crtc_start_addr_high;
+		uint8_t		crtc_start_addr_low;
+		uint16_t	crtc_start_addr;
+		uint8_t		crtc_cursor_loc_low;
+		uint8_t		crtc_cursor_loc_high;
+		uint16_t	crtc_cursor_loc;
+		uint8_t		crtc_vert_retrace_start;
+		uint8_t		crtc_vert_retrace_end;
+		uint8_t		crtc_vert_disp_end;
+		uint8_t		crtc_offset;
+		uint8_t		crtc_underline_loc;
+		uint8_t		crtc_start_vert_blank;
+		uint8_t		crtc_end_vert_blank;
+		uint8_t		crtc_line_compare;
+	} vga_crtc;
+
+	/*
+	 * Graphics Controller
+	 */
+	struct {
+		int		gc_index;
+		uint8_t		gc_set_reset;
+		uint8_t		gc_enb_set_reset;
+		uint8_t		gc_color_compare;
+		uint8_t		gc_rotate;
+		uint8_t		gc_op;
+		uint8_t		gc_read_map_sel;
+		uint8_t		gc_mode;
+		bool		gc_mode_c4;		/* chain 4 */
+		bool		gc_mode_oe;		/* odd/even */
+		uint8_t		gc_mode_rm;		/* read mode */
+		uint8_t		gc_mode_wm;		/* write mode */
+		uint8_t		gc_misc;
+		uint8_t		gc_misc_gm;		/* graphics mode */
+		uint8_t		gc_misc_mm;		/* memory map */
+		uint8_t		gc_color_dont_care;
+		uint8_t		gc_bit_mask;
+		uint8_t		gc_latch0;
+		uint8_t		gc_latch1;
+		uint8_t		gc_latch2;
+		uint8_t		gc_latch3;
+	} vga_gc;
+
+	/*
+	 * Attribute Controller
+	 */
+	struct {
+		int		atc_flipflop;
+		int		atc_index;
+		uint8_t		atc_palette[16];
+		uint8_t		atc_mode;
+		uint8_t		atc_overscan_color;
+		uint8_t		atc_color_plane_enb;
+		uint8_t		atc_horiz_pixel_panning;
+		uint8_t		atc_color_select;
+		uint8_t		atc_color_select_45;
+		uint8_t		atc_color_select_67;
+	} vga_atc;
+
+	/*
+	 * DAC
+	 */
+	struct {
+		uint8_t		dac_state;
+		int		dac_rd_index;
+		int		dac_rd_subindex;
+		int		dac_wr_index;
+		int		dac_wr_subindex;
+		uint8_t		dac_palette[3 * 256];
+		uint32_t	dac_palette_rgb[256];
+	} vga_dac;
+};
+
+static bool
+vga_in_reset(struct vga_softc *sc)
+{
+	return (((sc->vga_seq.seq_clock_mode & SEQ_CM_SO) != 0) ||
+	    ((sc->vga_seq.seq_reset & SEQ_RESET_ASYNC) == 0) ||
+	    ((sc->vga_seq.seq_reset & SEQ_RESET_SYNC) == 0) ||
+	    ((sc->vga_crtc.crtc_mode_ctrl & CRTC_MC_TE) == 0));
+}
+
+static void
+vga_check_size(struct bhyvegc *gc, struct vga_softc *sc)
+{
+	int old_width, old_height;
+
+	if (vga_in_reset(sc))
+		return;
+
+	//old_width = sc->gc_width;
+	//old_height = sc->gc_height;
+	old_width = sc->gc_image->width;
+	old_height = sc->gc_image->height;
+
+	/*
+	 * Horizontal Display End: For text modes this is the number
+	 * of characters.  For graphics modes this is the number of
+	 * pixels per scanlines divided by the number of pixels per
+	 * character clock.
+	 */
+	sc->gc_width = (sc->vga_crtc.crtc_horiz_disp_end + 1) *
+	    sc->vga_seq.seq_cm_dots;
+
+	sc->gc_height = (sc->vga_crtc.crtc_vert_disp_end |
+	    (((sc->vga_crtc.crtc_overflow & CRTC_OF_VDE8) >> CRTC_OF_VDE8_SHIFT) << 8) |
+	    (((sc->vga_crtc.crtc_overflow & CRTC_OF_VDE9) >> CRTC_OF_VDE9_SHIFT) << 9)) + 1;
+
+	if (old_width != sc->gc_width || old_height != sc->gc_height)
+		bhyvegc_resize(gc, sc->gc_width, sc->gc_height);
+}
+
+static uint32_t
+vga_get_pixel(struct vga_softc *sc, int x, int y)
+{
+	int offset;
+	int bit;
+	uint8_t data;
+	uint8_t idx;
+
+	offset = (y * sc->gc_width / 8) + (x / 8);
+	bit = 7 - (x % 8);
+
+	data = (((sc->vga_ram[offset + 0 * 64*KB] >> bit) & 0x1) << 0) |
+		(((sc->vga_ram[offset + 1 * 64*KB] >> bit) & 0x1) << 1) |
+		(((sc->vga_ram[offset + 2 * 64*KB] >> bit) & 0x1) << 2) |
+		(((sc->vga_ram[offset + 3 * 64*KB] >> bit) & 0x1) << 3);
+
+	data &= sc->vga_atc.atc_color_plane_enb;
+
+	if (sc->vga_atc.atc_mode & ATC_MC_IPS) {
+		idx = sc->vga_atc.atc_palette[data] & 0x0f;
+		idx |= sc->vga_atc.atc_color_select_45;
+	} else {
+		idx = sc->vga_atc.atc_palette[data];
+	}
+	idx |= sc->vga_atc.atc_color_select_67;
+
+	return (sc->vga_dac.dac_palette_rgb[idx]);
+}
+
+static void
+vga_render_graphics(struct vga_softc *sc)
+{
+	int x, y;
+
+	for (y = 0; y < sc->gc_height; y++) {
+		for (x = 0; x < sc->gc_width; x++) {
+			int offset;
+
+			offset = y * sc->gc_width + x;
+			sc->gc_image->data[offset] = vga_get_pixel(sc, x, y);
+		}
+	}
+}
+
+static uint32_t
+vga_get_text_pixel(struct vga_softc *sc, int x, int y)
+{
+	int dots, offset, bit, font_offset;
+	uint8_t ch, attr, font;
+	uint8_t idx;
+
+	dots = sc->vga_seq.seq_cm_dots;
+
+	offset = 2 * sc->vga_crtc.crtc_start_addr;
+	offset += (y / 16 * sc->gc_width / dots) * 2 + (x / dots) * 2;
+
+	bit = 7 - (x % dots > 7 ? 7 : x % dots);
+
+	ch = sc->vga_ram[offset + 0 * 64*KB];
+	attr = sc->vga_ram[offset + 1 * 64*KB];
+
+	if (sc->vga_crtc.crtc_cursor_on &&
+	    (offset == (sc->vga_crtc.crtc_cursor_loc * 2)) &&
+	    ((y % 16) >= (sc->vga_crtc.crtc_cursor_start & CRTC_CS_CS)) &&
+	    ((y % 16) <= (sc->vga_crtc.crtc_cursor_end & CRTC_CE_CE))) {
+		idx = sc->vga_atc.atc_palette[attr & 0xf];
+		return (sc->vga_dac.dac_palette_rgb[idx]);
+	}
+
+	if ((sc->vga_seq.seq_mm & SEQ_MM_EM) &&
+	    sc->vga_seq.seq_cmap_pri_off != sc->vga_seq.seq_cmap_sec_off) {
+		if (attr & 0x8)
+			font_offset = sc->vga_seq.seq_cmap_pri_off +
+				(ch << 5) + y % 16;
+		else
+			font_offset = sc->vga_seq.seq_cmap_sec_off +
+				(ch << 5) + y % 16;
+		attr &= ~0x8;
+	} else {
+		font_offset = (ch << 5) + y % 16;
+	}
+
+	font = sc->vga_ram[font_offset + 2 * 64*KB];
+
+	if (font & (1 << bit))
+		idx = sc->vga_atc.atc_palette[attr & 0xf];
+	else
+		idx = sc->vga_atc.atc_palette[attr >> 4];
+
+	return (sc->vga_dac.dac_palette_rgb[idx]);
+}
+
+static void
+vga_render_text(struct vga_softc *sc)
+{
+	int x, y;
+
+	for (y = 0; y < sc->gc_height; y++) {
+		for (x = 0; x < sc->gc_width; x++) {
+			int offset;
+
+			offset = y * sc->gc_width + x;
+			sc->gc_image->data[offset] = vga_get_text_pixel(sc, x, y);
+		}
+	}
+}
+
+void
+vga_render(struct bhyvegc *gc, void *arg)
+{
+	struct vga_softc *sc = arg;
+
+	vga_check_size(gc, sc);
+
+	if (vga_in_reset(sc)) {
+		memset(sc->gc_image->data, 0,
+		    sc->gc_image->width * sc->gc_image->height *
+		     sizeof (uint32_t));
+		return;
+	}
+
+	if (sc->vga_gc.gc_misc_gm && (sc->vga_atc.atc_mode & ATC_MC_GA))
+		vga_render_graphics(sc);
+	else
+		vga_render_text(sc);
+}
+
+static uint64_t
+vga_mem_rd_handler(struct vmctx *ctx, uint64_t addr, void *arg1)
+{
+	struct vga_softc *sc = arg1;
+	uint8_t map_sel;
+	int offset;
+
+	offset = addr;
+	switch (sc->vga_gc.gc_misc_mm) {
+	case 0x0:
+		/*
+		 * extended mode: base 0xa0000 size 128k
+		 */
+		offset -=0xa0000;
+		offset &= (128 * KB - 1);
+		break;
+	case 0x1:
+		/*
+		 * EGA/VGA mode: base 0xa0000 size 64k
+		 */
+		offset -=0xa0000;
+		offset &= (64 * KB - 1);
+		break;
+	case 0x2:
+		/*
+		 * monochrome text mode: base 0xb0000 size 32kb
+		 */
+		assert(0);
+	case 0x3:
+		/*
+		 * color text mode and CGA: base 0xb8000 size 32kb
+		 */
+		offset -=0xb8000;
+		offset &= (32 * KB - 1);
+		break;
+	}
+
+	/* Fill latches. */
+	sc->vga_gc.gc_latch0 = sc->vga_ram[offset + 0*64*KB];
+	sc->vga_gc.gc_latch1 = sc->vga_ram[offset + 1*64*KB];
+	sc->vga_gc.gc_latch2 = sc->vga_ram[offset + 2*64*KB];
+	sc->vga_gc.gc_latch3 = sc->vga_ram[offset + 3*64*KB];
+
+	if (sc->vga_gc.gc_mode_rm) {
+		/* read mode 1 */
+		assert(0);
+	}
+
+	map_sel = sc->vga_gc.gc_read_map_sel;
+	if (sc->vga_gc.gc_mode_oe) {
+		map_sel |= (offset & 1);
+		offset &= ~1;
+	}
+
+	/* read mode 0: return the byte from the selected plane. */
+	offset += map_sel * 64*KB;
+
+	return (sc->vga_ram[offset]);
+}
+
+static void
+vga_mem_wr_handler(struct vmctx *ctx, uint64_t addr, uint8_t val, void *arg1)
+{
+	struct vga_softc *sc = arg1;
+	uint8_t c0, c1, c2, c3;
+	uint8_t m0, m1, m2, m3;
+	uint8_t set_reset;
+	uint8_t enb_set_reset;
+	uint8_t	mask;
+	int offset;
+
+	offset = addr;
+	switch (sc->vga_gc.gc_misc_mm) {
+	case 0x0:
+		/*
+		 * extended mode: base 0xa0000 size 128kb
+		 */
+		offset -=0xa0000;
+		offset &= (128 * KB - 1);
+		break;
+	case 0x1:
+		/*
+		 * EGA/VGA mode: base 0xa0000 size 64kb
+		 */
+		offset -=0xa0000;
+		offset &= (64 * KB - 1);
+		break;
+	case 0x2:
+		/*
+		 * monochrome text mode: base 0xb0000 size 32kb
+		 */
+		assert(0);
+	case 0x3:
+		/*
+		 * color text mode and CGA: base 0xb8000 size 32kb
+		 */
+		offset -=0xb8000;
+		offset &= (32 * KB - 1);
+		break;
+	}
+
+	set_reset = sc->vga_gc.gc_set_reset;
+	enb_set_reset = sc->vga_gc.gc_enb_set_reset;
+
+	c0 = sc->vga_gc.gc_latch0;
+	c1 = sc->vga_gc.gc_latch1;
+	c2 = sc->vga_gc.gc_latch2;
+	c3 = sc->vga_gc.gc_latch3;
+
+	switch (sc->vga_gc.gc_mode_wm) {
+	case 0:
+		/* write mode 0 */
+		mask = sc->vga_gc.gc_bit_mask;
+
+		val = (val >> sc->vga_gc.gc_rotate) |
+		    (val << (8 - sc->vga_gc.gc_rotate));
+
+		switch (sc->vga_gc.gc_op) {
+		case 0x00:		/* replace */
+			m0 = (set_reset & 1) ? mask : 0x00;
+			m1 = (set_reset & 2) ? mask : 0x00;
+			m2 = (set_reset & 4) ? mask : 0x00;
+			m3 = (set_reset & 8) ? mask : 0x00;
+
+			c0 = (enb_set_reset & 1) ? (c0 & ~mask) : (val & mask);
+			c1 = (enb_set_reset & 2) ? (c1 & ~mask) : (val & mask);
+			c2 = (enb_set_reset & 4) ? (c2 & ~mask) : (val & mask);
+			c3 = (enb_set_reset & 8) ? (c3 & ~mask) : (val & mask);
+
+			c0 |= m0;
+			c1 |= m1;
+			c2 |= m2;
+			c3 |= m3;
+			break;
+		case 0x08:		/* AND */
+			m0 = set_reset & 1 ? 0xff : ~mask;
+			m1 = set_reset & 2 ? 0xff : ~mask;
+			m2 = set_reset & 4 ? 0xff : ~mask;
+			m3 = set_reset & 8 ? 0xff : ~mask;
+
+			c0 = enb_set_reset & 1 ? c0 & m0 : val & m0;
+			c1 = enb_set_reset & 2 ? c1 & m1 : val & m1;
+			c2 = enb_set_reset & 4 ? c2 & m2 : val & m2;
+			c3 = enb_set_reset & 8 ? c3 & m3 : val & m3;
+			break;
+		case 0x10:		/* OR */
+			m0 = set_reset & 1 ? mask : 0x00;
+			m1 = set_reset & 2 ? mask : 0x00;
+			m2 = set_reset & 4 ? mask : 0x00;
+			m3 = set_reset & 8 ? mask : 0x00;
+
+			c0 = enb_set_reset & 1 ? c0 | m0 : val | m0;
+			c1 = enb_set_reset & 2 ? c1 | m1 : val | m1;
+			c2 = enb_set_reset & 4 ? c2 | m2 : val | m2;
+			c3 = enb_set_reset & 8 ? c3 | m3 : val | m3;
+			break;
+		case 0x18:		/* XOR */
+			m0 = set_reset & 1 ? mask : 0x00;
+			m1 = set_reset & 2 ? mask : 0x00;
+			m2 = set_reset & 4 ? mask : 0x00;
+			m3 = set_reset & 8 ? mask : 0x00;
+
+			c0 = enb_set_reset & 1 ? c0 ^ m0 : val ^ m0;
+			c1 = enb_set_reset & 2 ? c1 ^ m1 : val ^ m1;
+			c2 = enb_set_reset & 4 ? c2 ^ m2 : val ^ m2;
+			c3 = enb_set_reset & 8 ? c3 ^ m3 : val ^ m3;
+			break;
+		}
+		break;
+	case 1:
+		/* write mode 1 */
+		break;
+	case 2:
+		/* write mode 2 */
+		mask = sc->vga_gc.gc_bit_mask;
+
+		switch (sc->vga_gc.gc_op) {
+		case 0x00:		/* replace */
+			m0 = (val & 1 ? 0xff : 0x00) & mask;
+			m1 = (val & 2 ? 0xff : 0x00) & mask;
+			m2 = (val & 4 ? 0xff : 0x00) & mask;
+			m3 = (val & 8 ? 0xff : 0x00) & mask;
+
+			c0 &= ~mask;
+			c1 &= ~mask;
+			c2 &= ~mask;
+			c3 &= ~mask;
+
+			c0 |= m0;
+			c1 |= m1;
+			c2 |= m2;
+			c3 |= m3;
+			break;
+		case 0x08:		/* AND */
+			m0 = (val & 1 ? 0xff : 0x00) | ~mask;
+			m1 = (val & 2 ? 0xff : 0x00) | ~mask;
+			m2 = (val & 4 ? 0xff : 0x00) | ~mask;
+			m3 = (val & 8 ? 0xff : 0x00) | ~mask;
+
+			c0 &= m0;
+			c1 &= m1;
+			c2 &= m2;
+			c3 &= m3;
+			break;
+		case 0x10:		/* OR */
+			m0 = (val & 1 ? 0xff : 0x00) & mask;
+			m1 = (val & 2 ? 0xff : 0x00) & mask;
+			m2 = (val & 4 ? 0xff : 0x00) & mask;
+			m3 = (val & 8 ? 0xff : 0x00) & mask;
+
+			c0 |= m0;
+			c1 |= m1;
+			c2 |= m2;
+			c3 |= m3;
+			break;
+		case 0x18:		/* XOR */
+			m0 = (val & 1 ? 0xff : 0x00) & mask;
+			m1 = (val & 2 ? 0xff : 0x00) & mask;
+			m2 = (val & 4 ? 0xff : 0x00) & mask;
+			m3 = (val & 8 ? 0xff : 0x00) & mask;
+
+			c0 ^= m0;
+			c1 ^= m1;
+			c2 ^= m2;
+			c3 ^= m3;
+			break;
+		}
+		break;
+	case 3:
+		/* write mode 3 */
+		mask = sc->vga_gc.gc_bit_mask & val;
+
+		val = (val >> sc->vga_gc.gc_rotate) |
+		    (val << (8 - sc->vga_gc.gc_rotate));
+
+		switch (sc->vga_gc.gc_op) {
+		case 0x00:		/* replace */
+			m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
+			m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
+			m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
+			m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
+
+			c0 &= ~mask;
+			c1 &= ~mask;
+			c2 &= ~mask;
+			c3 &= ~mask;
+
+			c0 |= m0;
+			c1 |= m1;
+			c2 |= m2;
+			c3 |= m3;
+			break;
+		case 0x08:		/* AND */
+			m0 = (set_reset & 1 ? 0xff : 0x00) | ~mask;
+			m1 = (set_reset & 2 ? 0xff : 0x00) | ~mask;
+			m2 = (set_reset & 4 ? 0xff : 0x00) | ~mask;
+			m3 = (set_reset & 8 ? 0xff : 0x00) | ~mask;
+
+			c0 &= m0;
+			c1 &= m1;
+			c2 &= m2;
+			c3 &= m3;
+			break;
+		case 0x10:		/* OR */
+			m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
+			m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
+			m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
+			m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
+
+			c0 |= m0;
+			c1 |= m1;
+			c2 |= m2;
+			c3 |= m3;
+			break;
+		case 0x18:		/* XOR */
+			m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
+			m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
+			m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
+			m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
+
+			c0 ^= m0;
+			c1 ^= m1;
+			c2 ^= m2;
+			c3 ^= m3;
+			break;
+		}
+		break;
+	}
+
+	if (sc->vga_gc.gc_mode_oe) {
+		if (offset & 1) {
+			offset &= ~1;
+			if (sc->vga_seq.seq_map_mask & 2)
+				sc->vga_ram[offset + 1*64*KB] = c1;
+			if (sc->vga_seq.seq_map_mask & 8)
+				sc->vga_ram[offset + 3*64*KB] = c3;
+		} else {
+			if (sc->vga_seq.seq_map_mask & 1)
+				sc->vga_ram[offset + 0*64*KB] = c0;
+			if (sc->vga_seq.seq_map_mask & 4)
+				sc->vga_ram[offset + 2*64*KB] = c2;
+		}
+	} else {
+		if (sc->vga_seq.seq_map_mask & 1)
+			sc->vga_ram[offset + 0*64*KB] = c0;
+		if (sc->vga_seq.seq_map_mask & 2)
+			sc->vga_ram[offset + 1*64*KB] = c1;
+		if (sc->vga_seq.seq_map_mask & 4)
+			sc->vga_ram[offset + 2*64*KB] = c2;
+		if (sc->vga_seq.seq_map_mask & 8)
+			sc->vga_ram[offset + 3*64*KB] = c3;
+	}
+}
+
+static int
+vga_mem_handler(struct vmctx *ctx, int vcpu, int dir, uint64_t addr,
+		int size, uint64_t *val, void *arg1, long arg2)
+{
+	if (dir == MEM_F_WRITE) {
+		switch (size) {
+		case 1:
+			vga_mem_wr_handler(ctx, addr, *val, arg1);
+			break;
+		case 2:
+			vga_mem_wr_handler(ctx, addr, *val, arg1);
+			vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
+			break;
+		case 4:
+			vga_mem_wr_handler(ctx, addr, *val, arg1);
+			vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
+			vga_mem_wr_handler(ctx, addr + 2, *val >> 16, arg1);
+			vga_mem_wr_handler(ctx, addr + 3, *val >> 24, arg1);
+			break;
+		case 8:
+			vga_mem_wr_handler(ctx, addr, *val, arg1);
+			vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
+			vga_mem_wr_handler(ctx, addr + 2, *val >> 16, arg1);
+			vga_mem_wr_handler(ctx, addr + 3, *val >> 24, arg1);
+			vga_mem_wr_handler(ctx, addr + 4, *val >> 32, arg1);
+			vga_mem_wr_handler(ctx, addr + 5, *val >> 40, arg1);
+			vga_mem_wr_handler(ctx, addr + 6, *val >> 48, arg1);
+			vga_mem_wr_handler(ctx, addr + 7, *val >> 56, arg1);
+			break;
+		}
+	} else {
+		switch (size) {
+		case 1:
+			*val = vga_mem_rd_handler(ctx, addr, arg1);
+			break;
+		case 2:
+			*val = vga_mem_rd_handler(ctx, addr, arg1);
+			*val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
+			break;
+		case 4:
+			*val = vga_mem_rd_handler(ctx, addr, arg1);
+			*val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
+			*val |= vga_mem_rd_handler(ctx, addr + 2, arg1) << 16;
+			*val |= vga_mem_rd_handler(ctx, addr + 3, arg1) << 24;
+			break;
+		case 8:
+			*val = vga_mem_rd_handler(ctx, addr, arg1);
+			*val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
+			*val |= vga_mem_rd_handler(ctx, addr + 2, arg1) << 16;
+			*val |= vga_mem_rd_handler(ctx, addr + 3, arg1) << 24;
+			*val |= vga_mem_rd_handler(ctx, addr + 4, arg1) << 32;
+			*val |= vga_mem_rd_handler(ctx, addr + 5, arg1) << 40;
+			*val |= vga_mem_rd_handler(ctx, addr + 6, arg1) << 48;
+			*val |= vga_mem_rd_handler(ctx, addr + 7, arg1) << 56;
+			break;
+		}
+	}
+
+	return (0);
+}
+
+static int
+vga_port_in_handler(struct vmctx *ctx, int in, int port, int bytes,
+		    uint8_t *val, void *arg)
+{
+	struct vga_softc *sc = arg;
+
+	switch (port) {
+	case CRTC_IDX_MONO_PORT:
+	case CRTC_IDX_COLOR_PORT:
+		*val = sc->vga_crtc.crtc_index;
+		break;
+	case CRTC_DATA_MONO_PORT:
+	case CRTC_DATA_COLOR_PORT:
+		switch (sc->vga_crtc.crtc_index) {
+		case CRTC_HORIZ_TOTAL:
+			*val = sc->vga_crtc.crtc_horiz_total;
+			break;
+		case CRTC_HORIZ_DISP_END:
+			*val = sc->vga_crtc.crtc_horiz_disp_end;
+			break;
+		case CRTC_START_HORIZ_BLANK:
+			*val = sc->vga_crtc.crtc_start_horiz_blank;
+			break;
+		case CRTC_END_HORIZ_BLANK:
+			*val = sc->vga_crtc.crtc_end_horiz_blank;
+			break;
+		case CRTC_START_HORIZ_RETRACE:
+			*val = sc->vga_crtc.crtc_start_horiz_retrace;
+			break;
+		case CRTC_END_HORIZ_RETRACE:
+			*val = sc->vga_crtc.crtc_end_horiz_retrace;
+			break;
+		case CRTC_VERT_TOTAL:
+			*val = sc->vga_crtc.crtc_vert_total;
+			break;
+		case CRTC_OVERFLOW:
+			*val = sc->vga_crtc.crtc_overflow;
+			break;
+		case CRTC_PRESET_ROW_SCAN:
+			*val = sc->vga_crtc.crtc_present_row_scan;
+			break;
+		case CRTC_MAX_SCAN_LINE:
+			*val = sc->vga_crtc.crtc_max_scan_line;
+			break;
+		case CRTC_CURSOR_START:
+			*val = sc->vga_crtc.crtc_cursor_start;
+			break;
+		case CRTC_CURSOR_END:
+			*val = sc->vga_crtc.crtc_cursor_end;
+			break;
+		case CRTC_START_ADDR_HIGH:
+			*val = sc->vga_crtc.crtc_start_addr_high;
+			break;
+		case CRTC_START_ADDR_LOW:
+			*val = sc->vga_crtc.crtc_start_addr_low;
+			break;
+		case CRTC_CURSOR_LOC_HIGH:
+			*val = sc->vga_crtc.crtc_cursor_loc_high;
+			break;
+		case CRTC_CURSOR_LOC_LOW:
+			*val = sc->vga_crtc.crtc_cursor_loc_low;
+			break;
+		case CRTC_VERT_RETRACE_START:
+			*val = sc->vga_crtc.crtc_vert_retrace_start;
+			break;
+		case CRTC_VERT_RETRACE_END:
+			*val = sc->vga_crtc.crtc_vert_retrace_end;
+			break;
+		case CRTC_VERT_DISP_END:
+			*val = sc->vga_crtc.crtc_vert_disp_end;
+			break;
+		case CRTC_OFFSET:
+			*val = sc->vga_crtc.crtc_offset;
+			break;
+		case CRTC_UNDERLINE_LOC:
+			*val = sc->vga_crtc.crtc_underline_loc;
+			break;
+		case CRTC_START_VERT_BLANK:
+			*val = sc->vga_crtc.crtc_start_vert_blank;
+			break;
+		case CRTC_END_VERT_BLANK:
+			*val = sc->vga_crtc.crtc_end_vert_blank;
+			break;
+		case CRTC_MODE_CONTROL:
+			*val = sc->vga_crtc.crtc_mode_ctrl;
+			break;
+		case CRTC_LINE_COMPARE:
+			*val = sc->vga_crtc.crtc_line_compare;
+			break;
+		default:
+			//printf("XXX VGA CRTC: inb 0x%04x at index %d\n", port, sc->vga_crtc.crtc_index);
+			assert(0);
+			break;
+		}
+		break;
+	case ATC_IDX_PORT:
+		*val = sc->vga_atc.atc_index;
+		break;
+	case ATC_DATA_PORT:
+		switch (sc->vga_atc.atc_index) {
+		case ATC_PALETTE0 ... ATC_PALETTE15:
+			*val = sc->vga_atc.atc_palette[sc->vga_atc.atc_index];
+			break;
+		case ATC_MODE_CONTROL:
+			*val = sc->vga_atc.atc_mode;
+			break;
+		case ATC_OVERSCAN_COLOR:
+			*val = sc->vga_atc.atc_overscan_color;
+			break;
+		case ATC_COLOR_PLANE_ENABLE:
+			*val = sc->vga_atc.atc_color_plane_enb;
+			break;
+		case ATC_HORIZ_PIXEL_PANNING:
+			*val = sc->vga_atc.atc_horiz_pixel_panning;
+			break;
+		case ATC_COLOR_SELECT:
+			*val = sc->vga_atc.atc_color_select;
+			break;
+		default:
+			//printf("XXX VGA ATC inb 0x%04x at index %d\n", port , sc->vga_atc.atc_index);
+			assert(0);
+			break;
+		}
+		break;
+	case SEQ_IDX_PORT:
+		*val = sc->vga_seq.seq_index;
+		break;
+	case SEQ_DATA_PORT:
+		switch (sc->vga_seq.seq_index) {
+		case SEQ_RESET:
+			*val = sc->vga_seq.seq_reset;
+			break;
+		case SEQ_CLOCKING_MODE:
+			*val = sc->vga_seq.seq_clock_mode;
+			break;
+		case SEQ_MAP_MASK:
+			*val = sc->vga_seq.seq_map_mask;
+			break;
+		case SEQ_CHAR_MAP_SELECT:
+			*val = sc->vga_seq.seq_cmap_sel;
+			break;
+		case SEQ_MEMORY_MODE:
+			*val = sc->vga_seq.seq_mm;
+			break;
+		default:
+			//printf("XXX VGA SEQ: inb 0x%04x at index %d\n", port, sc->vga_seq.seq_index);
+			assert(0);
+			break;
+		}
+		break;
+	case DAC_DATA_PORT:
+		*val = sc->vga_dac.dac_palette[3 * sc->vga_dac.dac_rd_index +
+					       sc->vga_dac.dac_rd_subindex];
+		sc->vga_dac.dac_rd_subindex++;
+		if (sc->vga_dac.dac_rd_subindex == 3) {
+			sc->vga_dac.dac_rd_index++;
+			sc->vga_dac.dac_rd_subindex = 0;
+		}
+		break;
+	case GC_IDX_PORT:
+		*val = sc->vga_gc.gc_index;
+		break;
+	case GC_DATA_PORT:
+		switch (sc->vga_gc.gc_index) {
+		case GC_SET_RESET:
+			*val = sc->vga_gc.gc_set_reset;
+			break;
+		case GC_ENABLE_SET_RESET:
+			*val = sc->vga_gc.gc_enb_set_reset;
+			break;
+		case GC_COLOR_COMPARE:
+			*val = sc->vga_gc.gc_color_compare;
+			break;
+		case GC_DATA_ROTATE:
+			*val = sc->vga_gc.gc_rotate;
+			break;
+		case GC_READ_MAP_SELECT:
+			*val = sc->vga_gc.gc_read_map_sel;
+			break;
+		case GC_MODE:
+			*val = sc->vga_gc.gc_mode;
+			break;
+		case GC_MISCELLANEOUS:
+			*val = sc->vga_gc.gc_misc;
+			break;
+		case GC_COLOR_DONT_CARE:
+			*val = sc->vga_gc.gc_color_dont_care;
+			break;
+		case GC_BIT_MASK:
+			*val = sc->vga_gc.gc_bit_mask;
+			break;
+		default:
+			//printf("XXX VGA GC: inb 0x%04x at index %d\n", port, sc->vga_crtc.crtc_index);
+			assert(0);
+			break;
+		}
+		break;
+	case GEN_MISC_OUTPUT_PORT:
+		*val = sc->vga_misc;
+		break;
+	case GEN_INPUT_STS0_PORT:
+		assert(0);
+		break;
+	case GEN_INPUT_STS1_MONO_PORT:
+	case GEN_INPUT_STS1_COLOR_PORT:
+		sc->vga_atc.atc_flipflop = 0;
+		sc->vga_sts1 = GEN_IS1_VR | GEN_IS1_DE;
+		//sc->vga_sts1 ^= (GEN_IS1_VR | GEN_IS1_DE);
+		*val = sc->vga_sts1;
+		break;
+	case GEN_FEATURE_CTRL_PORT:
+		// OpenBSD calls this with bytes = 1
+		//assert(0);
+		*val = 0;
+		break;
+	case 0x3c3:
+		*val = 0;
+		break;
+	default:
+		printf("XXX vga_port_in_handler() unhandled port 0x%x\n", port);
+		//assert(0);
+		return (-1);
+	}
+
+	return (0);
+}
+
+static int
+vga_port_out_handler(struct vmctx *ctx, int in, int port, int bytes,
+		     uint8_t val, void *arg)
+{
+	struct vga_softc *sc = arg;
+
+	switch (port) {
+	case CRTC_IDX_MONO_PORT:
+	case CRTC_IDX_COLOR_PORT:
+		sc->vga_crtc.crtc_index = val;
+		break;
+	case CRTC_DATA_MONO_PORT:
+	case CRTC_DATA_COLOR_PORT:
+		switch (sc->vga_crtc.crtc_index) {
+		case CRTC_HORIZ_TOTAL:
+			sc->vga_crtc.crtc_horiz_total = val;
+			break;
+		case CRTC_HORIZ_DISP_END:
+			sc->vga_crtc.crtc_horiz_disp_end = val;
+			break;
+		case CRTC_START_HORIZ_BLANK:
+			sc->vga_crtc.crtc_start_horiz_blank = val;
+			break;
+		case CRTC_END_HORIZ_BLANK:
+			sc->vga_crtc.crtc_end_horiz_blank = val;
+			break;
+		case CRTC_START_HORIZ_RETRACE:
+			sc->vga_crtc.crtc_start_horiz_retrace = val;
+			break;
+		case CRTC_END_HORIZ_RETRACE:
+			sc->vga_crtc.crtc_end_horiz_retrace = val;
+			break;
+		case CRTC_VERT_TOTAL:
+			sc->vga_crtc.crtc_vert_total = val;
+			break;
+		case CRTC_OVERFLOW:
+			sc->vga_crtc.crtc_overflow = val;
+			break;
+		case CRTC_PRESET_ROW_SCAN:
+			sc->vga_crtc.crtc_present_row_scan = val;
+			break;
+		case CRTC_MAX_SCAN_LINE:
+			sc->vga_crtc.crtc_max_scan_line = val;
+			break;
+		case CRTC_CURSOR_START:
+			sc->vga_crtc.crtc_cursor_start = val;
+			sc->vga_crtc.crtc_cursor_on = (val & CRTC_CS_CO) == 0;
+			break;
+		case CRTC_CURSOR_END:
+			sc->vga_crtc.crtc_cursor_end = val;
+			break;
+		case CRTC_START_ADDR_HIGH:
+			sc->vga_crtc.crtc_start_addr_high = val;
+			sc->vga_crtc.crtc_start_addr &= 0x00ff;
+			sc->vga_crtc.crtc_start_addr |= (val << 8);
+			break;
+		case CRTC_START_ADDR_LOW:
+			sc->vga_crtc.crtc_start_addr_low = val;
+			sc->vga_crtc.crtc_start_addr &= 0xff00;
+			sc->vga_crtc.crtc_start_addr |= (val & 0xff);
+			break;
+		case CRTC_CURSOR_LOC_HIGH:
+			sc->vga_crtc.crtc_cursor_loc_high = val;
+			sc->vga_crtc.crtc_cursor_loc &= 0x00ff;
+			sc->vga_crtc.crtc_cursor_loc |= (val << 8);
+			break;
+		case CRTC_CURSOR_LOC_LOW:
+			sc->vga_crtc.crtc_cursor_loc_low = val;
+			sc->vga_crtc.crtc_cursor_loc &= 0xff00;
+			sc->vga_crtc.crtc_cursor_loc |= (val & 0xff);
+			break;
+		case CRTC_VERT_RETRACE_START:
+			sc->vga_crtc.crtc_vert_retrace_start = val;
+			break;
+		case CRTC_VERT_RETRACE_END:
+			sc->vga_crtc.crtc_vert_retrace_end = val;
+			break;
+		case CRTC_VERT_DISP_END:
+			sc->vga_crtc.crtc_vert_disp_end = val;
+			break;
+		case CRTC_OFFSET:
+			sc->vga_crtc.crtc_offset = val;
+			break;
+		case CRTC_UNDERLINE_LOC:
+			sc->vga_crtc.crtc_underline_loc = val;
+			break;
+		case CRTC_START_VERT_BLANK:
+			sc->vga_crtc.crtc_start_vert_blank = val;
+			break;
+		case CRTC_END_VERT_BLANK:
+			sc->vga_crtc.crtc_end_vert_blank = val;
+			break;
+		case CRTC_MODE_CONTROL:
+			sc->vga_crtc.crtc_mode_ctrl = val;
+			break;
+		case CRTC_LINE_COMPARE:
+			sc->vga_crtc.crtc_line_compare = val;
+			break;
+		default:
+			//printf("XXX VGA CRTC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_crtc.crtc_index);
+			assert(0);
+			break;
+		}
+		break;
+	case ATC_IDX_PORT:
+		if (sc->vga_atc.atc_flipflop == 0) {
+			if (sc->vga_atc.atc_index & 0x20)
+				assert(0);
+			sc->vga_atc.atc_index = val & ATC_IDX_MASK;
+		} else {
+			switch (sc->vga_atc.atc_index) {
+			case ATC_PALETTE0 ... ATC_PALETTE15:
+				sc->vga_atc.atc_palette[sc->vga_atc.atc_index] = val & 0x3f;
+				break;
+			case ATC_MODE_CONTROL:
+				sc->vga_atc.atc_mode = val;
+				break;
+			case ATC_OVERSCAN_COLOR:
+				sc->vga_atc.atc_overscan_color = val;
+				break;
+			case ATC_COLOR_PLANE_ENABLE:
+				sc->vga_atc.atc_color_plane_enb = val;
+				break;
+			case ATC_HORIZ_PIXEL_PANNING:
+				sc->vga_atc.atc_horiz_pixel_panning = val;
+				break;
+			case ATC_COLOR_SELECT:
+				sc->vga_atc.atc_color_select = val;
+				sc->vga_atc.atc_color_select_45 =
+					(val & ATC_CS_C45) << 4;
+				sc->vga_atc.atc_color_select_67 =
+					(val & ATC_CS_C67) << 6;
+				break;
+			default:
+				//printf("XXX VGA ATC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_atc.atc_index);
+				assert(0);
+				break;
+			}
+		}
+		sc->vga_atc.atc_flipflop ^= 1;
+		break;
+	case ATC_DATA_PORT:
+		break;
+	case SEQ_IDX_PORT:
+		sc->vga_seq.seq_index = val & 0x1f;
+		break;
+	case SEQ_DATA_PORT:
+		switch (sc->vga_seq.seq_index) {
+		case SEQ_RESET:
+			sc->vga_seq.seq_reset = val;
+			break;
+		case SEQ_CLOCKING_MODE:
+			sc->vga_seq.seq_clock_mode = val;
+			sc->vga_seq.seq_cm_dots = (val & SEQ_CM_89) ? 8 : 9;
+			break;
+		case SEQ_MAP_MASK:
+			sc->vga_seq.seq_map_mask = val;
+			break;
+		case SEQ_CHAR_MAP_SELECT:
+			sc->vga_seq.seq_cmap_sel = val;
+
+			sc->vga_seq.seq_cmap_pri_off = ((((val & SEQ_CMS_SA) >> SEQ_CMS_SA_SHIFT) * 2) + ((val & SEQ_CMS_SAH) >> SEQ_CMS_SAH_SHIFT)) * 8 * KB;
+			sc->vga_seq.seq_cmap_sec_off = ((((val & SEQ_CMS_SB) >> SEQ_CMS_SB_SHIFT) * 2) + ((val & SEQ_CMS_SBH) >> SEQ_CMS_SBH_SHIFT)) * 8 * KB;
+			break;
+		case SEQ_MEMORY_MODE:
+			sc->vga_seq.seq_mm = val;
+			/* Windows queries Chain4 */
+			//assert((sc->vga_seq.seq_mm & SEQ_MM_C4) == 0);
+			break;
+		default:
+			//printf("XXX VGA SEQ: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_seq.seq_index);
+			assert(0);
+			break;
+		}
+		break;
+	case DAC_MASK:
+		break;
+	case DAC_IDX_RD_PORT:
+		sc->vga_dac.dac_rd_index = val;
+		sc->vga_dac.dac_rd_subindex = 0;
+		break;
+	case DAC_IDX_WR_PORT:
+		sc->vga_dac.dac_wr_index = val;
+		sc->vga_dac.dac_wr_subindex = 0;
+		break;
+	case DAC_DATA_PORT:
+		sc->vga_dac.dac_palette[3 * sc->vga_dac.dac_wr_index +
+					sc->vga_dac.dac_wr_subindex] = val;
+		sc->vga_dac.dac_wr_subindex++;
+		if (sc->vga_dac.dac_wr_subindex == 3) {
+			sc->vga_dac.dac_palette_rgb[sc->vga_dac.dac_wr_index] =
+				((((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] << 2) |
+				   ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] & 0x1) << 1) |
+				   (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] & 0x1)) << 16) |
+				 (((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] << 2) |
+				   ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] & 0x1) << 1) |
+				   (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] & 0x1)) << 8) |
+				 (((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] << 2) |
+				   ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] & 0x1) << 1) |
+				   (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] & 0x1)) << 0));
+
+			sc->vga_dac.dac_wr_index++;
+			sc->vga_dac.dac_wr_subindex = 0;
+		}
+		break;
+	case GC_IDX_PORT:
+		sc->vga_gc.gc_index = val;
+		break;
+	case GC_DATA_PORT:
+		switch (sc->vga_gc.gc_index) {
+		case GC_SET_RESET:
+			sc->vga_gc.gc_set_reset = val;
+			break;
+		case GC_ENABLE_SET_RESET:
+			sc->vga_gc.gc_enb_set_reset = val;
+			break;
+		case GC_COLOR_COMPARE:
+			sc->vga_gc.gc_color_compare = val;
+			break;
+		case GC_DATA_ROTATE:
+			sc->vga_gc.gc_rotate = val;
+			sc->vga_gc.gc_op = (val >> 3) & 0x3;
+			break;
+		case GC_READ_MAP_SELECT:
+			sc->vga_gc.gc_read_map_sel = val;
+			break;
+		case GC_MODE:
+			sc->vga_gc.gc_mode = val;
+			sc->vga_gc.gc_mode_c4 = (val & GC_MODE_C4) != 0;
+			assert(!sc->vga_gc.gc_mode_c4);
+			sc->vga_gc.gc_mode_oe = (val & GC_MODE_OE) != 0;
+			sc->vga_gc.gc_mode_rm = (val >> 3) & 0x1;
+			sc->vga_gc.gc_mode_wm = val & 0x3;
+
+			if (sc->gc_image)
+				sc->gc_image->vgamode = 1;
+			break;
+		case GC_MISCELLANEOUS:
+			sc->vga_gc.gc_misc = val;
+			sc->vga_gc.gc_misc_gm = val & GC_MISC_GM;
+			sc->vga_gc.gc_misc_mm = (val & GC_MISC_MM) >>
+			    GC_MISC_MM_SHIFT;
+			break;
+		case GC_COLOR_DONT_CARE:
+			sc->vga_gc.gc_color_dont_care = val;
+			break;
+		case GC_BIT_MASK:
+			sc->vga_gc.gc_bit_mask = val;
+			break;
+		default:
+			//printf("XXX VGA GC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_gc.gc_index);
+			assert(0);
+			break;
+		}
+		break;
+	case GEN_INPUT_STS0_PORT:
+		/* write to Miscellaneous Output Register */
+		sc->vga_misc = val;
+		break;
+	case GEN_INPUT_STS1_MONO_PORT:
+	case GEN_INPUT_STS1_COLOR_PORT:
+		/* write to Feature Control Register */
+		break;
+//	case 0x3c3:
+//		break;
+	default:
+		printf("XXX vga_port_out_handler() unhandled port 0x%x, val 0x%x\n", port, val);
+		//assert(0);
+		return (-1);
+	}
+	return (0);
+}
+
+static int
+vga_port_handler(struct vmctx *ctx, int vcpu, int in, int port, int bytes,
+		 uint32_t *eax, void *arg)
+{
+	uint8_t val;
+	int error;
+
+	switch (bytes) {
+	case 1:
+		if (in) {
+			*eax &= ~0xff;
+			error = vga_port_in_handler(ctx, in, port, 1,
+						    &val, arg);
+			if (!error) {
+				*eax |= val & 0xff;
+			}
+		} else {
+			val = *eax & 0xff;
+			error = vga_port_out_handler(ctx, in, port, 1,
+						     val, arg);
+		}
+		break;
+	case 2:
+		if (in) {
+			*eax &= ~0xffff;
+			error = vga_port_in_handler(ctx, in, port, 1,
+						    &val, arg);
+			if (!error) {
+				*eax |= val & 0xff;
+			}
+			error = vga_port_in_handler(ctx, in, port + 1, 1,
+						    &val, arg);
+			if (!error) {
+				*eax |= (val & 0xff) << 8;
+			}
+		} else {
+			val = *eax & 0xff;
+			error = vga_port_out_handler(ctx, in, port, 1,
+						     val, arg);
+			val = (*eax >> 8) & 0xff;
+			error =vga_port_out_handler(ctx, in, port + 1, 1,
+						    val, arg);
+		}
+		break;
+	default:
+		assert(0);
+		return (-1);
+	}
+
+	return (error);
+}
+
+void *
+vga_init(int io_only)
+{
+	struct inout_port iop;
+	struct vga_softc *sc;
+	int port, error;
+
+	sc = calloc(1, sizeof(struct vga_softc));
+
+	bzero(&iop, sizeof(struct inout_port));
+	iop.name = "VGA";
+	for (port = VGA_IOPORT_START; port <= VGA_IOPORT_END; port++) {
+		iop.port = port;
+		iop.size = 1;
+		iop.flags = IOPORT_F_INOUT;
+		iop.handler = vga_port_handler;
+		iop.arg = sc;
+
+		error = register_inout(&iop);
+		assert(error == 0);
+	}
+
+	sc->gc_image = console_get_image();
+
+	/* only handle io ports; vga graphics is disabled */
+	if (io_only)
+		return(sc);
+
+	sc->mr.name = "VGA memory";
+	sc->mr.flags = MEM_F_RW;
+	sc->mr.base = 640 * KB;
+	sc->mr.size = 128 * KB;
+	sc->mr.handler = vga_mem_handler;
+	sc->mr.arg1 = sc;
+	error = register_mem_fallback(&sc->mr);
+	assert(error == 0);
+
+	sc->vga_ram = malloc(256 * KB);
+	memset(sc->vga_ram, 0, 256 * KB);
+
+	{
+		static uint8_t palette[] = {
+			0x00,0x00,0x00, 0x00,0x00,0x2a, 0x00,0x2a,0x00, 0x00,0x2a,0x2a,
+			0x2a,0x00,0x00, 0x2a,0x00,0x2a, 0x2a,0x2a,0x00, 0x2a,0x2a,0x2a,
+			0x00,0x00,0x15, 0x00,0x00,0x3f, 0x00,0x2a,0x15, 0x00,0x2a,0x3f,
+			0x2a,0x00,0x15, 0x2a,0x00,0x3f, 0x2a,0x2a,0x15, 0x2a,0x2a,0x3f,
+		};
+		int i;
+
+		memcpy(sc->vga_dac.dac_palette, palette, 16 * 3 * sizeof (uint8_t));
+		for (i = 0; i < 16; i++) {
+			sc->vga_dac.dac_palette_rgb[i] =
+				((((sc->vga_dac.dac_palette[3*i + 0] << 2) |
+				   ((sc->vga_dac.dac_palette[3*i + 0] & 0x1) << 1) |
+				   (sc->vga_dac.dac_palette[3*i + 0] & 0x1)) << 16) |
+				 (((sc->vga_dac.dac_palette[3*i + 1] << 2) |
+				   ((sc->vga_dac.dac_palette[3*i + 1] & 0x1) << 1) |
+				   (sc->vga_dac.dac_palette[3*i + 1] & 0x1)) << 8) |
+				 (((sc->vga_dac.dac_palette[3*i + 2] << 2) |
+				   ((sc->vga_dac.dac_palette[3*i + 2] & 0x1) << 1) |
+				   (sc->vga_dac.dac_palette[3*i + 2] & 0x1)) << 0));
+		}
+	}
+
+	return (sc);
+}
diff --git a/vga.h b/vga.h
new file mode 100644
index 000000000000..4364f1b17a81
--- /dev/null
+++ b/vga.h
@@ -0,0 +1,160 @@
+/*-
+ * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
+ * 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 ``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$
+ */
+
+#ifndef _VGA_H_
+#define	_VGA_H_
+
+#define	VGA_IOPORT_START		0x3c0
+#define	VGA_IOPORT_END			0x3df
+
+/* General registers */
+#define	GEN_INPUT_STS0_PORT		0x3c2
+#define	GEN_FEATURE_CTRL_PORT		0x3ca
+#define	GEN_MISC_OUTPUT_PORT		0x3cc
+#define	GEN_INPUT_STS1_MONO_PORT	0x3ba
+#define	GEN_INPUT_STS1_COLOR_PORT	0x3da
+#define	 GEN_IS1_VR			0x08	/* Vertical retrace */
+#define	 GEN_IS1_DE			0x01	/* Display enable not */
+
+/* Attribute controller registers. */
+#define	ATC_IDX_PORT			0x3c0
+#define	ATC_DATA_PORT			0x3c1
+
+#define	ATC_IDX_MASK			0x1f
+#define	ATC_PALETTE0			0
+#define	ATC_PALETTE15			15
+#define	ATC_MODE_CONTROL		16
+#define	 ATC_MC_IPS			0x80	/* Internal palette size */
+#define	 ATC_MC_GA			0x01	/* Graphics/alphanumeric */
+#define	ATC_OVERSCAN_COLOR		17
+#define	ATC_COLOR_PLANE_ENABLE		18
+#define	ATC_HORIZ_PIXEL_PANNING		19
+#define	ATC_COLOR_SELECT		20
+#define	 ATC_CS_C67			0x0c	/* Color select bits 6+7 */
+#define	 ATC_CS_C45			0x03	/* Color select bits 4+5 */
+
+/* Sequencer registers. */
+#define	SEQ_IDX_PORT			0x3c4
+#define	SEQ_DATA_PORT			0x3c5
+
+#define	SEQ_RESET			0
+#define	SEQ_RESET_ASYNC			0x1
+#define	SEQ_RESET_SYNC			0x2
+#define	SEQ_CLOCKING_MODE		1
+#define	 SEQ_CM_SO			0x20	/* Screen off */
+#define	 SEQ_CM_89			0x01	/* 8/9 dot clock */
+#define	SEQ_MAP_MASK			2
+#define	SEQ_CHAR_MAP_SELECT		3
+#define	 SEQ_CMS_SAH			0x20	/* Char map A bit 2 */
+#define	 SEQ_CMS_SAH_SHIFT		5
+#define	 SEQ_CMS_SA			0x0c	/* Char map A bits 0+1 */
+#define	 SEQ_CMS_SA_SHIFT		2
+#define	 SEQ_CMS_SBH			0x10	/* Char map B bit 2 */
+#define	 SEQ_CMS_SBH_SHIFT		4
+#define	 SEQ_CMS_SB			0x03	/* Char map B bits 0+1 */
+#define	 SEQ_CMS_SB_SHIFT		0
+#define	SEQ_MEMORY_MODE			4
+#define	 SEQ_MM_C4			0x08	/* Chain 4 */
+#define	 SEQ_MM_OE			0x04	/* Odd/even */
+#define	 SEQ_MM_EM			0x02	/* Extended memory */
+
+/* Graphics controller registers. */
+#define	GC_IDX_PORT			0x3ce
+#define	GC_DATA_PORT			0x3cf
+
+#define	GC_SET_RESET			0
+#define	GC_ENABLE_SET_RESET		1
+#define	GC_COLOR_COMPARE		2
+#define	GC_DATA_ROTATE			3
+#define	GC_READ_MAP_SELECT		4
+#define	GC_MODE				5
+#define	 GC_MODE_OE			0x10	/* Odd/even */
+#define	 GC_MODE_C4			0x04	/* Chain 4 */
+
+#define	GC_MISCELLANEOUS		6
+#define	 GC_MISC_GM			0x01	/* Graphics/alphanumeric */
+#define	 GC_MISC_MM			0x0c	/* memory map */
+#define	 GC_MISC_MM_SHIFT	2
+#define	GC_COLOR_DONT_CARE		7
+#define	GC_BIT_MASK			8
+
+/* CRT controller registers. */
+#define	CRTC_IDX_MONO_PORT		0x3b4
+#define	CRTC_DATA_MONO_PORT		0x3b5
+#define	CRTC_IDX_COLOR_PORT		0x3d4
+#define	CRTC_DATA_COLOR_PORT		0x3d5
+
+#define	CRTC_HORIZ_TOTAL		0
+#define	CRTC_HORIZ_DISP_END		1
+#define	CRTC_START_HORIZ_BLANK		2
+#define	CRTC_END_HORIZ_BLANK		3
+#define	CRTC_START_HORIZ_RETRACE	4
+#define	CRTC_END_HORIZ_RETRACE		5
+#define	CRTC_VERT_TOTAL			6
+#define	CRTC_OVERFLOW			7
+#define	 CRTC_OF_VRS9			0x80	/* VRS bit 9 */
+#define	 CRTC_OF_VRS9_SHIFT		7
+#define	 CRTC_OF_VDE9			0x40	/* VDE bit 9 */
+#define	 CRTC_OF_VDE9_SHIFT		6
+#define	 CRTC_OF_VRS8			0x04	/* VRS bit 8 */
+#define	 CRTC_OF_VRS8_SHIFT		2
+#define	 CRTC_OF_VDE8			0x02	/* VDE bit 8 */
+#define	 CRTC_OF_VDE8_SHIFT		1
+#define	CRTC_PRESET_ROW_SCAN		8
+#define	CRTC_MAX_SCAN_LINE		9
+#define	 CRTC_MSL_MSL			0x1f
+#define	CRTC_CURSOR_START		10
+#define	 CRTC_CS_CO			0x20	/* Cursor off */
+#define	 CRTC_CS_CS			0x1f	/* Cursor start */
+#define	CRTC_CURSOR_END			11
+#define	 CRTC_CE_CE			0x1f	/* Cursor end */
+#define	CRTC_START_ADDR_HIGH		12
+#define	CRTC_START_ADDR_LOW		13
+#define	CRTC_CURSOR_LOC_HIGH		14
+#define	CRTC_CURSOR_LOC_LOW		15
+#define	CRTC_VERT_RETRACE_START		16
+#define	CRTC_VERT_RETRACE_END		17
+#define	 CRTC_VRE_MASK			0xf
+#define	CRTC_VERT_DISP_END		18
+#define	CRTC_OFFSET			19
+#define	CRTC_UNDERLINE_LOC		20
+#define	CRTC_START_VERT_BLANK		21
+#define	CRTC_END_VERT_BLANK		22
+#define	CRTC_MODE_CONTROL		23
+#define	 CRTC_MC_TE			0x80	/* Timing enable */
+#define	CRTC_LINE_COMPARE		24
+
+/* DAC registers */
+#define	DAC_MASK			0x3c6
+#define	DAC_IDX_RD_PORT			0x3c7
+#define	DAC_IDX_WR_PORT			0x3c8
+#define	DAC_DATA_PORT			0x3c9
+
+void	*vga_init(int io_only);
+
+#endif /* _VGA_H_ */