From 205b961b15c4de4789f484ddd795b6a42d1b1eac Mon Sep 17 00:00:00 2001 From: Peter Grehan Date: Fri, 27 May 2016 06:30:35 +0000 Subject: [PATCH] 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. --- Makefile | 15 +- atkbdc.c | 525 +++++++++- atkbdc.h | 38 + bhyvegc.c | 72 ++ bhyvegc.h | 46 + bhyverun.c | 2 + console.c | 118 +++ console.h | 53 + pci_fbuf.c | 406 ++++++++ pci_xhci.c | 2828 ++++++++++++++++++++++++++++++++++++++++++++++++++ pci_xhci.h | 353 +++++++ ps2kbd.c | 481 +++++++++ ps2kbd.h | 39 + ps2mouse.c | 405 ++++++++ ps2mouse.h | 41 + rfb.c | 949 +++++++++++++++++ rfb.h | 36 + sockstream.c | 86 ++ sockstream.h | 33 + usb_emul.c | 76 ++ usb_emul.h | 156 +++ usb_mouse.c | 803 ++++++++++++++ vga.c | 1331 ++++++++++++++++++++++++ vga.h | 160 +++ 24 files changed, 9035 insertions(+), 17 deletions(-) create mode 100644 atkbdc.h create mode 100644 bhyvegc.c create mode 100644 bhyvegc.h create mode 100644 console.c create mode 100644 console.h create mode 100644 pci_fbuf.c create mode 100644 pci_xhci.c create mode 100644 pci_xhci.h create mode 100644 ps2kbd.c create mode 100644 ps2kbd.h create mode 100644 ps2mouse.c create mode 100644 ps2mouse.h create mode 100644 rfb.c create mode 100644 rfb.h create mode 100644 sockstream.c create mode 100644 sockstream.h create mode 100644 usb_emul.c create mode 100644 usb_emul.h create mode 100644 usb_mouse.c create mode 100644 vga.c create mode 100644 vga.h diff --git a/Makefile b/Makefile index 7d2e2f0ed17..f010f3d408d 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 930b7af95f3..8e71b0507ca 100644 --- a/atkbdc.c +++ b/atkbdc.c @@ -1,5 +1,6 @@ /*- * Copyright (c) 2014 Tycho Nightingale + * 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 #include +#include +#include #include +#include +#include +#include +#include +#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; - *eax = 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; + } - return (0); + 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; + } + + sc->curcmd = 0; + sc->status &= ~KBDS_CTRL_FLAG; + + pthread_mutex_unlock(&sc->mtx); + return (retval); + } + + /* + * 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 00000000000..85c8a7141eb --- /dev/null +++ b/atkbdc.h @@ -0,0 +1,38 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 00000000000..377b05b0878 --- /dev/null +++ b/bhyvegc.c @@ -0,0 +1,72 @@ +#include + +#include + +#include +#include +#include + +#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 00000000000..fa2ab68d9e2 --- /dev/null +++ b/bhyvegc.h @@ -0,0 +1,46 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 bfa135b16cc..e62dc688730 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 00000000000..ebb9c921bf0 --- /dev/null +++ b/console.c @@ -0,0 +1,118 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 +__FBSDID("$FreeBSD$"); + +#include + +#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 00000000000..47193938a6f --- /dev/null +++ b/console.h @@ -0,0 +1,53 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 00000000000..22ec86c5bd3 --- /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 +__FBSDID("$FreeBSD$"); + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#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 ,fbuf,wait,tcp=: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=: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 00000000000..416226821f6 --- /dev/null +++ b/pci_xhci.c @@ -0,0 +1,2828 @@ +/*- + * Copyright (c) 2014 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 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 ,xhci,{devices} + + devices: + ums USB tablet mouse + */ +#include +__FBSDID("$FreeBSD$"); + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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[=] */ + 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 00000000000..d5f05af5d02 --- /dev/null +++ b/pci_xhci.h @@ -0,0 +1,353 @@ +/*- + * Copyright (c) 2014 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 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 00000000000..aa4aeae03ef --- /dev/null +++ b/ps2kbd.c @@ -0,0 +1,481 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 +__FBSDID("$FreeBSD$"); + +#include + +#include +#include +#include +#include +#include +#include +#include + +#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 00000000000..34c31b1ea88 --- /dev/null +++ b/ps2kbd.h @@ -0,0 +1,39 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 00000000000..e207a939a37 --- /dev/null +++ b/ps2mouse.c @@ -0,0 +1,405 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 +__FBSDID("$FreeBSD$"); + +#include + +#include +#include +#include +#include +#include +#include +#include + +#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 00000000000..10d5698a308 --- /dev/null +++ b/ps2mouse.h @@ -0,0 +1,41 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 00000000000..ceeb1b75450 --- /dev/null +++ b/rfb.c @@ -0,0 +1,949 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 +__FBSDID("$FreeBSD$"); + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#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 00000000000..b3d3786669c --- /dev/null +++ b/rfb.h @@ -0,0 +1,36 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 00000000000..1789206ff39 --- /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 +__FBSDID("$FreeBSD$"); + +#include +#include + +#include + +#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 00000000000..bb0b3b06eb9 --- /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 +#include + +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 00000000000..3dc12a5c3c1 --- /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 +__FBSDID("$FreeBSD$"); + +#include +#include + +#include +#include +#include +#include +#include + +#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 00000000000..69df135466b --- /dev/null +++ b/usb_emul.h @@ -0,0 +1,156 @@ +/*- + * Copyright (c) 2014 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 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 +#include +#include + +#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 00000000000..c2f21eb4feb --- /dev/null +++ b/usb_mouse.c @@ -0,0 +1,803 @@ +/*- + * Copyright (c) 2014 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 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 +__FBSDID("$FreeBSD$"); + +#include + +#include +#include +#include +#include + +#include +#include + +#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 00000000000..208064b3b2f --- /dev/null +++ b/vga.c @@ -0,0 +1,1331 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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 +__FBSDID("$FreeBSD$"); + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#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 00000000000..4364f1b17a8 --- /dev/null +++ b/vga.h @@ -0,0 +1,160 @@ +/*- + * Copyright (c) 2015 Tycho Nightingale + * 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_ */