usr.sbin/bhyve/vga.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   3  *
   4  * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
   5  * All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions and the following disclaimer.
  12  * 2. Redistributions in binary form must reproduce the above copyright
  13  *    notice, this list of conditions and the following disclaimer in the
  14  *    documentation and/or other materials provided with the distribution.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND
  17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  19  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  20  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  21  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  22  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  24  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  25  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  26  * SUCH DAMAGE.
  27  */
  28
  29 #include <sys/cdefs.h>
  30 __FBSDID("$FreeBSD$");
  31
  32 #include <sys/param.h>
  33
  34 #include <assert.h>
  35 #include <pthread.h>
  36 #include <stdbool.h>
  37 #include <stdio.h>
  38 #include <stdlib.h>
  39 #include <string.h>
  40
  41 #include <machine/vmm.h>
  42
  43 #include "bhyvegc.h"
  44 #include "console.h"
  45 #include "inout.h"
  46 #include "mem.h"
  47 #include "vga.h"
  48
  49 #define KB      (1024UL)
  50 #define MB      (1024 * 1024UL)
  51
  52 struct vga_softc {
  53         struct mem_range        mr;
  54
  55         struct bhyvegc          *gc;
  56         int                     gc_width;
  57         int                     gc_height;
  58         struct bhyvegc_image    *gc_image;
  59
  60         uint8_t                 *vga_ram;
  61
  62         /*
  63          * General registers
  64          */
  65         uint8_t                 vga_misc;
  66         uint8_t                 vga_sts1;
  67
  68         /*
  69          * Sequencer
  70          */
  71         struct {
  72                 int             seq_index;
  73                 uint8_t         seq_reset;
  74                 uint8_t         seq_clock_mode;
  75                 int             seq_cm_dots;
  76                 uint8_t         seq_map_mask;
  77                 uint8_t         seq_cmap_sel;
  78                 int             seq_cmap_pri_off;
  79                 int             seq_cmap_sec_off;
  80                 uint8_t         seq_mm;
  81         } vga_seq;
  82
  83         /*
  84          * CRT Controller
  85          */
  86         struct {
  87                 int             crtc_index;
  88                 uint8_t         crtc_mode_ctrl;
  89                 uint8_t         crtc_horiz_total;
  90                 uint8_t         crtc_horiz_disp_end;
  91                 uint8_t         crtc_start_horiz_blank;
  92                 uint8_t         crtc_end_horiz_blank;
  93                 uint8_t         crtc_start_horiz_retrace;
  94                 uint8_t         crtc_end_horiz_retrace;
  95                 uint8_t         crtc_vert_total;
  96                 uint8_t         crtc_overflow;
  97                 uint8_t         crtc_present_row_scan;
  98                 uint8_t         crtc_max_scan_line;
  99                 uint8_t         crtc_cursor_start;
 100                 uint8_t         crtc_cursor_on;
 101                 uint8_t         crtc_cursor_end;
 102                 uint8_t         crtc_start_addr_high;
 103                 uint8_t         crtc_start_addr_low;
 104                 uint16_t        crtc_start_addr;
 105                 uint8_t         crtc_cursor_loc_low;
 106                 uint8_t         crtc_cursor_loc_high;
 107                 uint16_t        crtc_cursor_loc;
 108                 uint8_t         crtc_vert_retrace_start;
 109                 uint8_t         crtc_vert_retrace_end;
 110                 uint8_t         crtc_vert_disp_end;
 111                 uint8_t         crtc_offset;
 112                 uint8_t         crtc_underline_loc;
 113                 uint8_t         crtc_start_vert_blank;
 114                 uint8_t         crtc_end_vert_blank;
 115                 uint8_t         crtc_line_compare;
 116         } vga_crtc;
 117
 118         /*
 119          * Graphics Controller
 120          */
 121         struct {
 122                 int             gc_index;
 123                 uint8_t         gc_set_reset;
 124                 uint8_t         gc_enb_set_reset;
 125                 uint8_t         gc_color_compare;
 126                 uint8_t         gc_rotate;
 127                 uint8_t         gc_op;
 128                 uint8_t         gc_read_map_sel;
 129                 uint8_t         gc_mode;
 130                 bool            gc_mode_c4;             /* chain 4 */
 131                 bool            gc_mode_oe;             /* odd/even */
 132                 uint8_t         gc_mode_rm;             /* read mode */
 133                 uint8_t         gc_mode_wm;             /* write mode */
 134                 uint8_t         gc_misc;
 135                 uint8_t         gc_misc_gm;             /* graphics mode */
 136                 uint8_t         gc_misc_mm;             /* memory map */
 137                 uint8_t         gc_color_dont_care;
 138                 uint8_t         gc_bit_mask;
 139                 uint8_t         gc_latch0;
 140                 uint8_t         gc_latch1;
 141                 uint8_t         gc_latch2;
 142                 uint8_t         gc_latch3;
 143         } vga_gc;
 144
 145         /*
 146          * Attribute Controller
 147          */
 148         struct {
 149                 int             atc_flipflop;
 150                 int             atc_index;
 151                 uint8_t         atc_palette[16];
 152                 uint8_t         atc_mode;
 153                 uint8_t         atc_overscan_color;
 154                 uint8_t         atc_color_plane_enb;
 155                 uint8_t         atc_horiz_pixel_panning;
 156                 uint8_t         atc_color_select;
 157                 uint8_t         atc_color_select_45;
 158                 uint8_t         atc_color_select_67;
 159         } vga_atc;
 160
 161         /*
 162          * DAC
 163          */
 164         struct {
 165                 uint8_t         dac_state;
 166                 uint8_t         dac_rd_index;
 167                 uint8_t         dac_rd_subindex;
 168                 uint8_t         dac_wr_index;
 169                 uint8_t         dac_wr_subindex;
 170                 uint8_t         dac_palette[3 * 256];
 171                 uint32_t        dac_palette_rgb[256];
 172         } vga_dac;
 173 };
 174
 175 static bool
 176 vga_in_reset(struct vga_softc *sc)
 177 {
 178         return (((sc->vga_seq.seq_clock_mode & SEQ_CM_SO) != 0) ||
 179             ((sc->vga_seq.seq_reset & SEQ_RESET_ASYNC) == 0) ||
 180             ((sc->vga_seq.seq_reset & SEQ_RESET_SYNC) == 0) ||
 181             ((sc->vga_crtc.crtc_mode_ctrl & CRTC_MC_TE) == 0));
 182 }
 183
 184 static void
 185 vga_check_size(struct bhyvegc *gc, struct vga_softc *sc)
 186 {
 187         int old_width, old_height;
 188
 189         if (vga_in_reset(sc))
 190                 return;
 191
 192         //old_width = sc->gc_width;
 193         //old_height = sc->gc_height;
 194         old_width = sc->gc_image->width;
 195         old_height = sc->gc_image->height;
 196
 197         /*
 198          * Horizontal Display End: For text modes this is the number
 199          * of characters.  For graphics modes this is the number of
 200          * pixels per scanlines divided by the number of pixels per
 201          * character clock.
 202          */
 203         sc->gc_width = (sc->vga_crtc.crtc_horiz_disp_end + 1) *
 204             sc->vga_seq.seq_cm_dots;
 205
 206         sc->gc_height = (sc->vga_crtc.crtc_vert_disp_end |
 207             (((sc->vga_crtc.crtc_overflow & CRTC_OF_VDE8) >> CRTC_OF_VDE8_SHIFT) << 8) |
 208             (((sc->vga_crtc.crtc_overflow & CRTC_OF_VDE9) >> CRTC_OF_VDE9_SHIFT) << 9)) + 1;
 209
 210         if (old_width != sc->gc_width || old_height != sc->gc_height)
 211                 bhyvegc_resize(gc, sc->gc_width, sc->gc_height);
 212 }
 213
 214 static uint32_t
 215 vga_get_pixel(struct vga_softc *sc, int x, int y)
 216 {
 217         int offset;
 218         int bit;
 219         uint8_t data;
 220         uint8_t idx;
 221
 222         offset = (y * sc->gc_width / 8) + (x / 8);
 223         bit = 7 - (x % 8);
 224
 225         data = (((sc->vga_ram[offset + 0 * 64*KB] >> bit) & 0x1) << 0) |
 226                 (((sc->vga_ram[offset + 1 * 64*KB] >> bit) & 0x1) << 1) |
 227                 (((sc->vga_ram[offset + 2 * 64*KB] >> bit) & 0x1) << 2) |
 228                 (((sc->vga_ram[offset + 3 * 64*KB] >> bit) & 0x1) << 3);
 229
 230         data &= sc->vga_atc.atc_color_plane_enb;
 231
 232         if (sc->vga_atc.atc_mode & ATC_MC_IPS) {
 233                 idx = sc->vga_atc.atc_palette[data] & 0x0f;
 234                 idx |= sc->vga_atc.atc_color_select_45;
 235         } else {
 236                 idx = sc->vga_atc.atc_palette[data];
 237         }
 238         idx |= sc->vga_atc.atc_color_select_67;
 239
 240         return (sc->vga_dac.dac_palette_rgb[idx]);
 241 }
 242
 243 static void
 244 vga_render_graphics(struct vga_softc *sc)
 245 {
 246         int x, y;
 247
 248         for (y = 0; y < sc->gc_height; y++) {
 249                 for (x = 0; x < sc->gc_width; x++) {
 250                         int offset;
 251
 252                         offset = y * sc->gc_width + x;
 253                         sc->gc_image->data[offset] = vga_get_pixel(sc, x, y);
 254                 }
 255         }
 256 }
 257
 258 static uint32_t
 259 vga_get_text_pixel(struct vga_softc *sc, int x, int y)
 260 {
 261         int dots, offset, bit, font_offset;
 262         uint8_t ch, attr, font;
 263         uint8_t idx;
 264
 265         dots = sc->vga_seq.seq_cm_dots;
 266
 267         offset = 2 * sc->vga_crtc.crtc_start_addr;
 268         offset += (y / 16 * sc->gc_width / dots) * 2 + (x / dots) * 2;
 269
 270         bit = 7 - (x % dots > 7 ? 7 : x % dots);
 271
 272         ch = sc->vga_ram[offset + 0 * 64*KB];
 273         attr = sc->vga_ram[offset + 1 * 64*KB];
 274
 275         if (sc->vga_crtc.crtc_cursor_on &&
 276             (offset == (sc->vga_crtc.crtc_cursor_loc * 2)) &&
 277             ((y % 16) >= (sc->vga_crtc.crtc_cursor_start & CRTC_CS_CS)) &&
 278             ((y % 16) <= (sc->vga_crtc.crtc_cursor_end & CRTC_CE_CE))) {
 279                 idx = sc->vga_atc.atc_palette[attr & 0xf];
 280                 return (sc->vga_dac.dac_palette_rgb[idx]);
 281         }
 282
 283         if ((sc->vga_seq.seq_mm & SEQ_MM_EM) &&
 284             sc->vga_seq.seq_cmap_pri_off != sc->vga_seq.seq_cmap_sec_off) {
 285                 if (attr & 0x8)
 286                         font_offset = sc->vga_seq.seq_cmap_pri_off +
 287                                 (ch << 5) + y % 16;
 288                 else
 289                         font_offset = sc->vga_seq.seq_cmap_sec_off +
 290                                 (ch << 5) + y % 16;
 291                 attr &= ~0x8;
 292         } else {
 293                 font_offset = (ch << 5) + y % 16;
 294         }
 295
 296         font = sc->vga_ram[font_offset + 2 * 64*KB];
 297
 298         if (font & (1 << bit))
 299                 idx = sc->vga_atc.atc_palette[attr & 0xf];
 300         else
 301                 idx = sc->vga_atc.atc_palette[attr >> 4];
 302
 303         return (sc->vga_dac.dac_palette_rgb[idx]);
 304 }
 305
 306 static void
 307 vga_render_text(struct vga_softc *sc)
 308 {
 309         int x, y;
 310
 311         for (y = 0; y < sc->gc_height; y++) {
 312                 for (x = 0; x < sc->gc_width; x++) {
 313                         int offset;
 314
 315                         offset = y * sc->gc_width + x;
 316                         sc->gc_image->data[offset] = vga_get_text_pixel(sc, x, y);
 317                 }
 318         }
 319 }
 320
 321 void
 322 vga_render(struct bhyvegc *gc, void *arg)
 323 {
 324         struct vga_softc *sc = arg;
 325
 326         vga_check_size(gc, sc);
 327
 328         if (vga_in_reset(sc)) {
 329                 memset(sc->gc_image->data, 0,
 330                     sc->gc_image->width * sc->gc_image->height *
 331                      sizeof (uint32_t));
 332                 return;
 333         }
 334
 335         if (sc->vga_gc.gc_misc_gm && (sc->vga_atc.atc_mode & ATC_MC_GA))
 336                 vga_render_graphics(sc);
 337         else
 338                 vga_render_text(sc);
 339 }
 340
 341 static uint64_t
 342 vga_mem_rd_handler(struct vmctx *ctx, uint64_t addr, void *arg1)
 343 {
 344         struct vga_softc *sc = arg1;
 345         uint8_t map_sel;
 346         int offset;
 347
 348         offset = addr;
 349         switch (sc->vga_gc.gc_misc_mm) {
 350         case 0x0:
 351                 /*
 352                  * extended mode: base 0xa0000 size 128k
 353                  */
 354                 offset -=0xa0000;
 355                 offset &= (128 * KB - 1);
 356                 break;
 357         case 0x1:
 358                 /*
 359                  * EGA/VGA mode: base 0xa0000 size 64k
 360                  */
 361                 offset -=0xa0000;
 362                 offset &= (64 * KB - 1);
 363                 break;
 364         case 0x2:
 365                 /*
 366                  * monochrome text mode: base 0xb0000 size 32kb
 367                  */
 368                 assert(0);
 369         case 0x3:
 370                 /*
 371                  * color text mode and CGA: base 0xb8000 size 32kb
 372                  */
 373                 offset -=0xb8000;
 374                 offset &= (32 * KB - 1);
 375                 break;
 376         }
 377
 378         /* Fill latches. */
 379         sc->vga_gc.gc_latch0 = sc->vga_ram[offset + 0*64*KB];
 380         sc->vga_gc.gc_latch1 = sc->vga_ram[offset + 1*64*KB];
 381         sc->vga_gc.gc_latch2 = sc->vga_ram[offset + 2*64*KB];
 382         sc->vga_gc.gc_latch3 = sc->vga_ram[offset + 3*64*KB];
 383
 384         if (sc->vga_gc.gc_mode_rm) {
 385                 /* read mode 1 */
 386                 assert(0);
 387         }
 388
 389         map_sel = sc->vga_gc.gc_read_map_sel;
 390         if (sc->vga_gc.gc_mode_oe) {
 391                 map_sel |= (offset & 1);
 392                 offset &= ~1;
 393         }
 394
 395         /* read mode 0: return the byte from the selected plane. */
 396         offset += map_sel * 64*KB;
 397
 398         return (sc->vga_ram[offset]);
 399 }
 400
 401 static void
 402 vga_mem_wr_handler(struct vmctx *ctx, uint64_t addr, uint8_t val, void *arg1)
 403 {
 404         struct vga_softc *sc = arg1;
 405         uint8_t c0, c1, c2, c3;
 406         uint8_t m0, m1, m2, m3;
 407         uint8_t set_reset;
 408         uint8_t enb_set_reset;
 409         uint8_t mask;
 410         int offset;
 411
 412         offset = addr;
 413         switch (sc->vga_gc.gc_misc_mm) {
 414         case 0x0:
 415                 /*
 416                  * extended mode: base 0xa0000 size 128kb
 417                  */
 418                 offset -=0xa0000;
 419                 offset &= (128 * KB - 1);
 420                 break;
 421         case 0x1:
 422                 /*
 423                  * EGA/VGA mode: base 0xa0000 size 64kb
 424                  */
 425                 offset -=0xa0000;
 426                 offset &= (64 * KB - 1);
 427                 break;
 428         case 0x2:
 429                 /*
 430                  * monochrome text mode: base 0xb0000 size 32kb
 431                  */
 432                 assert(0);
 433         case 0x3:
 434                 /*
 435                  * color text mode and CGA: base 0xb8000 size 32kb
 436                  */
 437                 offset -=0xb8000;
 438                 offset &= (32 * KB - 1);
 439                 break;
 440         }
 441
 442         set_reset = sc->vga_gc.gc_set_reset;
 443         enb_set_reset = sc->vga_gc.gc_enb_set_reset;
 444
 445         c0 = sc->vga_gc.gc_latch0;
 446         c1 = sc->vga_gc.gc_latch1;
 447         c2 = sc->vga_gc.gc_latch2;
 448         c3 = sc->vga_gc.gc_latch3;
 449
 450         switch (sc->vga_gc.gc_mode_wm) {
 451         case 0:
 452                 /* write mode 0 */
 453                 mask = sc->vga_gc.gc_bit_mask;
 454
 455                 val = (val >> sc->vga_gc.gc_rotate) |
 456                     (val << (8 - sc->vga_gc.gc_rotate));
 457
 458                 switch (sc->vga_gc.gc_op) {
 459                 case 0x00:              /* replace */
 460                         m0 = (set_reset & 1) ? mask : 0x00;
 461                         m1 = (set_reset & 2) ? mask : 0x00;
 462                         m2 = (set_reset & 4) ? mask : 0x00;
 463                         m3 = (set_reset & 8) ? mask : 0x00;
 464
 465                         c0 = (enb_set_reset & 1) ? (c0 & ~mask) : (val & mask);
 466                         c1 = (enb_set_reset & 2) ? (c1 & ~mask) : (val & mask);
 467                         c2 = (enb_set_reset & 4) ? (c2 & ~mask) : (val & mask);
 468                         c3 = (enb_set_reset & 8) ? (c3 & ~mask) : (val & mask);
 469
 470                         c0 |= m0;
 471                         c1 |= m1;
 472                         c2 |= m2;
 473                         c3 |= m3;
 474                         break;
 475                 case 0x08:              /* AND */
 476                         m0 = set_reset & 1 ? 0xff : ~mask;
 477                         m1 = set_reset & 2 ? 0xff : ~mask;
 478                         m2 = set_reset & 4 ? 0xff : ~mask;
 479                         m3 = set_reset & 8 ? 0xff : ~mask;
 480
 481                         c0 = enb_set_reset & 1 ? c0 & m0 : val & m0;
 482                         c1 = enb_set_reset & 2 ? c1 & m1 : val & m1;
 483                         c2 = enb_set_reset & 4 ? c2 & m2 : val & m2;
 484                         c3 = enb_set_reset & 8 ? c3 & m3 : val & m3;
 485                         break;
 486                 case 0x10:              /* OR */
 487                         m0 = set_reset & 1 ? mask : 0x00;
 488                         m1 = set_reset & 2 ? mask : 0x00;
 489                         m2 = set_reset & 4 ? mask : 0x00;
 490                         m3 = set_reset & 8 ? mask : 0x00;
 491
 492                         c0 = enb_set_reset & 1 ? c0 | m0 : val | m0;
 493                         c1 = enb_set_reset & 2 ? c1 | m1 : val | m1;
 494                         c2 = enb_set_reset & 4 ? c2 | m2 : val | m2;
 495                         c3 = enb_set_reset & 8 ? c3 | m3 : val | m3;
 496                         break;
 497                 case 0x18:              /* XOR */
 498                         m0 = set_reset & 1 ? mask : 0x00;
 499                         m1 = set_reset & 2 ? mask : 0x00;
 500                         m2 = set_reset & 4 ? mask : 0x00;
 501                         m3 = set_reset & 8 ? mask : 0x00;
 502
 503                         c0 = enb_set_reset & 1 ? c0 ^ m0 : val ^ m0;
 504                         c1 = enb_set_reset & 2 ? c1 ^ m1 : val ^ m1;
 505                         c2 = enb_set_reset & 4 ? c2 ^ m2 : val ^ m2;
 506                         c3 = enb_set_reset & 8 ? c3 ^ m3 : val ^ m3;
 507                         break;
 508                 }
 509                 break;
 510         case 1:
 511                 /* write mode 1 */
 512                 break;
 513         case 2:
 514                 /* write mode 2 */
 515                 mask = sc->vga_gc.gc_bit_mask;
 516
 517                 switch (sc->vga_gc.gc_op) {
 518                 case 0x00:              /* replace */
 519                         m0 = (val & 1 ? 0xff : 0x00) & mask;
 520                         m1 = (val & 2 ? 0xff : 0x00) & mask;
 521                         m2 = (val & 4 ? 0xff : 0x00) & mask;
 522                         m3 = (val & 8 ? 0xff : 0x00) & mask;
 523
 524                         c0 &= ~mask;
 525                         c1 &= ~mask;
 526                         c2 &= ~mask;
 527                         c3 &= ~mask;
 528
 529                         c0 |= m0;
 530                         c1 |= m1;
 531                         c2 |= m2;
 532                         c3 |= m3;
 533                         break;
 534                 case 0x08:              /* AND */
 535                         m0 = (val & 1 ? 0xff : 0x00) | ~mask;
 536                         m1 = (val & 2 ? 0xff : 0x00) | ~mask;
 537                         m2 = (val & 4 ? 0xff : 0x00) | ~mask;
 538                         m3 = (val & 8 ? 0xff : 0x00) | ~mask;
 539
 540                         c0 &= m0;
 541                         c1 &= m1;
 542                         c2 &= m2;
 543                         c3 &= m3;
 544                         break;
 545                 case 0x10:              /* OR */
 546                         m0 = (val & 1 ? 0xff : 0x00) & mask;
 547                         m1 = (val & 2 ? 0xff : 0x00) & mask;
 548                         m2 = (val & 4 ? 0xff : 0x00) & mask;
 549                         m3 = (val & 8 ? 0xff : 0x00) & mask;
 550
 551                         c0 |= m0;
 552                         c1 |= m1;
 553                         c2 |= m2;
 554                         c3 |= m3;
 555                         break;
 556                 case 0x18:              /* XOR */
 557                         m0 = (val & 1 ? 0xff : 0x00) & mask;
 558                         m1 = (val & 2 ? 0xff : 0x00) & mask;
 559                         m2 = (val & 4 ? 0xff : 0x00) & mask;
 560                         m3 = (val & 8 ? 0xff : 0x00) & mask;
 561
 562                         c0 ^= m0;
 563                         c1 ^= m1;
 564                         c2 ^= m2;
 565                         c3 ^= m3;
 566                         break;
 567                 }
 568                 break;
 569         case 3:
 570                 /* write mode 3 */
 571                 mask = sc->vga_gc.gc_bit_mask & val;
 572
 573                 val = (val >> sc->vga_gc.gc_rotate) |
 574                     (val << (8 - sc->vga_gc.gc_rotate));
 575
 576                 switch (sc->vga_gc.gc_op) {
 577                 case 0x00:              /* replace */
 578                         m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
 579                         m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
 580                         m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
 581                         m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
 582
 583                         c0 &= ~mask;
 584                         c1 &= ~mask;
 585                         c2 &= ~mask;
 586                         c3 &= ~mask;
 587
 588                         c0 |= m0;
 589                         c1 |= m1;
 590                         c2 |= m2;
 591                         c3 |= m3;
 592                         break;
 593                 case 0x08:              /* AND */
 594                         m0 = (set_reset & 1 ? 0xff : 0x00) | ~mask;
 595                         m1 = (set_reset & 2 ? 0xff : 0x00) | ~mask;
 596                         m2 = (set_reset & 4 ? 0xff : 0x00) | ~mask;
 597                         m3 = (set_reset & 8 ? 0xff : 0x00) | ~mask;
 598
 599                         c0 &= m0;
 600                         c1 &= m1;
 601                         c2 &= m2;
 602                         c3 &= m3;
 603                         break;
 604                 case 0x10:              /* OR */
 605                         m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
 606                         m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
 607                         m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
 608                         m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
 609
 610                         c0 |= m0;
 611                         c1 |= m1;
 612                         c2 |= m2;
 613                         c3 |= m3;
 614                         break;
 615                 case 0x18:              /* XOR */
 616                         m0 = (set_reset & 1 ? 0xff : 0x00) & mask;
 617                         m1 = (set_reset & 2 ? 0xff : 0x00) & mask;
 618                         m2 = (set_reset & 4 ? 0xff : 0x00) & mask;
 619                         m3 = (set_reset & 8 ? 0xff : 0x00) & mask;
 620
 621                         c0 ^= m0;
 622                         c1 ^= m1;
 623                         c2 ^= m2;
 624                         c3 ^= m3;
 625                         break;
 626                 }
 627                 break;
 628         }
 629
 630         if (sc->vga_gc.gc_mode_oe) {
 631                 if (offset & 1) {
 632                         offset &= ~1;
 633                         if (sc->vga_seq.seq_map_mask & 2)
 634                                 sc->vga_ram[offset + 1*64*KB] = c1;
 635                         if (sc->vga_seq.seq_map_mask & 8)
 636                                 sc->vga_ram[offset + 3*64*KB] = c3;
 637                 } else {
 638                         if (sc->vga_seq.seq_map_mask & 1)
 639                                 sc->vga_ram[offset + 0*64*KB] = c0;
 640                         if (sc->vga_seq.seq_map_mask & 4)
 641                                 sc->vga_ram[offset + 2*64*KB] = c2;
 642                 }
 643         } else {
 644                 if (sc->vga_seq.seq_map_mask & 1)
 645                         sc->vga_ram[offset + 0*64*KB] = c0;
 646                 if (sc->vga_seq.seq_map_mask & 2)
 647                         sc->vga_ram[offset + 1*64*KB] = c1;
 648                 if (sc->vga_seq.seq_map_mask & 4)
 649                         sc->vga_ram[offset + 2*64*KB] = c2;
 650                 if (sc->vga_seq.seq_map_mask & 8)
 651                         sc->vga_ram[offset + 3*64*KB] = c3;
 652         }
 653 }
 654
 655 static int
 656 vga_mem_handler(struct vmctx *ctx, int vcpu, int dir, uint64_t addr,
 657                 int size, uint64_t *val, void *arg1, long arg2)
 658 {
 659         if (dir == MEM_F_WRITE) {
 660                 switch (size) {
 661                 case 1:
 662                         vga_mem_wr_handler(ctx, addr, *val, arg1);
 663                         break;
 664                 case 2:
 665                         vga_mem_wr_handler(ctx, addr, *val, arg1);
 666                         vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
 667                         break;
 668                 case 4:
 669                         vga_mem_wr_handler(ctx, addr, *val, arg1);
 670                         vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
 671                         vga_mem_wr_handler(ctx, addr + 2, *val >> 16, arg1);
 672                         vga_mem_wr_handler(ctx, addr + 3, *val >> 24, arg1);
 673                         break;
 674                 case 8:
 675                         vga_mem_wr_handler(ctx, addr, *val, arg1);
 676                         vga_mem_wr_handler(ctx, addr + 1, *val >> 8, arg1);
 677                         vga_mem_wr_handler(ctx, addr + 2, *val >> 16, arg1);
 678                         vga_mem_wr_handler(ctx, addr + 3, *val >> 24, arg1);
 679                         vga_mem_wr_handler(ctx, addr + 4, *val >> 32, arg1);
 680                         vga_mem_wr_handler(ctx, addr + 5, *val >> 40, arg1);
 681                         vga_mem_wr_handler(ctx, addr + 6, *val >> 48, arg1);
 682                         vga_mem_wr_handler(ctx, addr + 7, *val >> 56, arg1);
 683                         break;
 684                 }
 685         } else {
 686                 switch (size) {
 687                 case 1:
 688                         *val = vga_mem_rd_handler(ctx, addr, arg1);
 689                         break;
 690                 case 2:
 691                         *val = vga_mem_rd_handler(ctx, addr, arg1);
 692                         *val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
 693                         break;
 694                 case 4:
 695                         *val = vga_mem_rd_handler(ctx, addr, arg1);
 696                         *val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
 697                         *val |= vga_mem_rd_handler(ctx, addr + 2, arg1) << 16;
 698                         *val |= vga_mem_rd_handler(ctx, addr + 3, arg1) << 24;
 699                         break;
 700                 case 8:
 701                         *val = vga_mem_rd_handler(ctx, addr, arg1);
 702                         *val |= vga_mem_rd_handler(ctx, addr + 1, arg1) << 8;
 703                         *val |= vga_mem_rd_handler(ctx, addr + 2, arg1) << 16;
 704                         *val |= vga_mem_rd_handler(ctx, addr + 3, arg1) << 24;
 705                         *val |= vga_mem_rd_handler(ctx, addr + 4, arg1) << 32;
 706                         *val |= vga_mem_rd_handler(ctx, addr + 5, arg1) << 40;
 707                         *val |= vga_mem_rd_handler(ctx, addr + 6, arg1) << 48;
 708                         *val |= vga_mem_rd_handler(ctx, addr + 7, arg1) << 56;
 709                         break;
 710                 }
 711         }
 712
 713         return (0);
 714 }
 715
 716 static int
 717 vga_port_in_handler(struct vmctx *ctx, int in, int port, int bytes,
 718                     uint8_t *val, void *arg)
 719 {
 720         struct vga_softc *sc = arg;
 721
 722         switch (port) {
 723         case CRTC_IDX_MONO_PORT:
 724         case CRTC_IDX_COLOR_PORT:
 725                 *val = sc->vga_crtc.crtc_index;
 726                 break;
 727         case CRTC_DATA_MONO_PORT:
 728         case CRTC_DATA_COLOR_PORT:
 729                 switch (sc->vga_crtc.crtc_index) {
 730                 case CRTC_HORIZ_TOTAL:
 731                         *val = sc->vga_crtc.crtc_horiz_total;
 732                         break;
 733                 case CRTC_HORIZ_DISP_END:
 734                         *val = sc->vga_crtc.crtc_horiz_disp_end;
 735                         break;
 736                 case CRTC_START_HORIZ_BLANK:
 737                         *val = sc->vga_crtc.crtc_start_horiz_blank;
 738                         break;
 739                 case CRTC_END_HORIZ_BLANK:
 740                         *val = sc->vga_crtc.crtc_end_horiz_blank;
 741                         break;
 742                 case CRTC_START_HORIZ_RETRACE:
 743                         *val = sc->vga_crtc.crtc_start_horiz_retrace;
 744                         break;
 745                 case CRTC_END_HORIZ_RETRACE:
 746                         *val = sc->vga_crtc.crtc_end_horiz_retrace;
 747                         break;
 748                 case CRTC_VERT_TOTAL:
 749                         *val = sc->vga_crtc.crtc_vert_total;
 750                         break;
 751                 case CRTC_OVERFLOW:
 752                         *val = sc->vga_crtc.crtc_overflow;
 753                         break;
 754                 case CRTC_PRESET_ROW_SCAN:
 755                         *val = sc->vga_crtc.crtc_present_row_scan;
 756                         break;
 757                 case CRTC_MAX_SCAN_LINE:
 758                         *val = sc->vga_crtc.crtc_max_scan_line;
 759                         break;
 760                 case CRTC_CURSOR_START:
 761                         *val = sc->vga_crtc.crtc_cursor_start;
 762                         break;
 763                 case CRTC_CURSOR_END:
 764                         *val = sc->vga_crtc.crtc_cursor_end;
 765                         break;
 766                 case CRTC_START_ADDR_HIGH:
 767                         *val = sc->vga_crtc.crtc_start_addr_high;
 768                         break;
 769                 case CRTC_START_ADDR_LOW:
 770                         *val = sc->vga_crtc.crtc_start_addr_low;
 771                         break;
 772                 case CRTC_CURSOR_LOC_HIGH:
 773                         *val = sc->vga_crtc.crtc_cursor_loc_high;
 774                         break;
 775                 case CRTC_CURSOR_LOC_LOW:
 776                         *val = sc->vga_crtc.crtc_cursor_loc_low;
 777                         break;
 778                 case CRTC_VERT_RETRACE_START:
 779                         *val = sc->vga_crtc.crtc_vert_retrace_start;
 780                         break;
 781                 case CRTC_VERT_RETRACE_END:
 782                         *val = sc->vga_crtc.crtc_vert_retrace_end;
 783                         break;
 784                 case CRTC_VERT_DISP_END:
 785                         *val = sc->vga_crtc.crtc_vert_disp_end;
 786                         break;
 787                 case CRTC_OFFSET:
 788                         *val = sc->vga_crtc.crtc_offset;
 789                         break;
 790                 case CRTC_UNDERLINE_LOC:
 791                         *val = sc->vga_crtc.crtc_underline_loc;
 792                         break;
 793                 case CRTC_START_VERT_BLANK:
 794                         *val = sc->vga_crtc.crtc_start_vert_blank;
 795                         break;
 796                 case CRTC_END_VERT_BLANK:
 797                         *val = sc->vga_crtc.crtc_end_vert_blank;
 798                         break;
 799                 case CRTC_MODE_CONTROL:
 800                         *val = sc->vga_crtc.crtc_mode_ctrl;
 801                         break;
 802                 case CRTC_LINE_COMPARE:
 803                         *val = sc->vga_crtc.crtc_line_compare;
 804                         break;
 805                 default:
 806                         //printf("XXX VGA CRTC: inb 0x%04x at index %d\n", port, sc->vga_crtc.crtc_index);
 807                         assert(0);
 808                         break;
 809                 }
 810                 break;
 811         case ATC_IDX_PORT:
 812                 *val = sc->vga_atc.atc_index;
 813                 break;
 814         case ATC_DATA_PORT:
 815                 switch (sc->vga_atc.atc_index) {
 816                 case ATC_PALETTE0 ... ATC_PALETTE15:
 817                         *val = sc->vga_atc.atc_palette[sc->vga_atc.atc_index];
 818                         break;
 819                 case ATC_MODE_CONTROL:
 820                         *val = sc->vga_atc.atc_mode;
 821                         break;
 822                 case ATC_OVERSCAN_COLOR:
 823                         *val = sc->vga_atc.atc_overscan_color;
 824                         break;
 825                 case ATC_COLOR_PLANE_ENABLE:
 826                         *val = sc->vga_atc.atc_color_plane_enb;
 827                         break;
 828                 case ATC_HORIZ_PIXEL_PANNING:
 829                         *val = sc->vga_atc.atc_horiz_pixel_panning;
 830                         break;
 831                 case ATC_COLOR_SELECT:
 832                         *val = sc->vga_atc.atc_color_select;
 833                         break;
 834                 default:
 835                         //printf("XXX VGA ATC inb 0x%04x at index %d\n", port , sc->vga_atc.atc_index);
 836                         assert(0);
 837                         break;
 838                 }
 839                 break;
 840         case SEQ_IDX_PORT:
 841                 *val = sc->vga_seq.seq_index;
 842                 break;
 843         case SEQ_DATA_PORT:
 844                 switch (sc->vga_seq.seq_index) {
 845                 case SEQ_RESET:
 846                         *val = sc->vga_seq.seq_reset;
 847                         break;
 848                 case SEQ_CLOCKING_MODE:
 849                         *val = sc->vga_seq.seq_clock_mode;
 850                         break;
 851                 case SEQ_MAP_MASK:
 852                         *val = sc->vga_seq.seq_map_mask;
 853                         break;
 854                 case SEQ_CHAR_MAP_SELECT:
 855                         *val = sc->vga_seq.seq_cmap_sel;
 856                         break;
 857                 case SEQ_MEMORY_MODE:
 858                         *val = sc->vga_seq.seq_mm;
 859                         break;
 860                 default:
 861                         //printf("XXX VGA SEQ: inb 0x%04x at index %d\n", port, sc->vga_seq.seq_index);
 862                         assert(0);
 863                         break;
 864                 }
 865                 break;
 866         case DAC_DATA_PORT:
 867                 *val = sc->vga_dac.dac_palette[3 * sc->vga_dac.dac_rd_index +
 868                                                sc->vga_dac.dac_rd_subindex];
 869                 sc->vga_dac.dac_rd_subindex++;
 870                 if (sc->vga_dac.dac_rd_subindex == 3) {
 871                         sc->vga_dac.dac_rd_index++;
 872                         sc->vga_dac.dac_rd_subindex = 0;
 873                 }
 874                 break;
 875         case GC_IDX_PORT:
 876                 *val = sc->vga_gc.gc_index;
 877                 break;
 878         case GC_DATA_PORT:
 879                 switch (sc->vga_gc.gc_index) {
 880                 case GC_SET_RESET:
 881                         *val = sc->vga_gc.gc_set_reset;
 882                         break;
 883                 case GC_ENABLE_SET_RESET:
 884                         *val = sc->vga_gc.gc_enb_set_reset;
 885                         break;
 886                 case GC_COLOR_COMPARE:
 887                         *val = sc->vga_gc.gc_color_compare;
 888                         break;
 889                 case GC_DATA_ROTATE:
 890                         *val = sc->vga_gc.gc_rotate;
 891                         break;
 892                 case GC_READ_MAP_SELECT:
 893                         *val = sc->vga_gc.gc_read_map_sel;
 894                         break;
 895                 case GC_MODE:
 896                         *val = sc->vga_gc.gc_mode;
 897                         break;
 898                 case GC_MISCELLANEOUS:
 899                         *val = sc->vga_gc.gc_misc;
 900                         break;
 901                 case GC_COLOR_DONT_CARE:
 902                         *val = sc->vga_gc.gc_color_dont_care;
 903                         break;
 904                 case GC_BIT_MASK:
 905                         *val = sc->vga_gc.gc_bit_mask;
 906                         break;
 907                 default:
 908                         //printf("XXX VGA GC: inb 0x%04x at index %d\n", port, sc->vga_crtc.crtc_index);
 909                         assert(0);
 910                         break;
 911                 }
 912                 break;
 913         case GEN_MISC_OUTPUT_PORT:
 914                 *val = sc->vga_misc;
 915                 break;
 916         case GEN_INPUT_STS0_PORT:
 917                 assert(0);
 918                 break;
 919         case GEN_INPUT_STS1_MONO_PORT:
 920         case GEN_INPUT_STS1_COLOR_PORT:
 921                 sc->vga_atc.atc_flipflop = 0;
 922                 sc->vga_sts1 = GEN_IS1_VR | GEN_IS1_DE;
 923                 //sc->vga_sts1 ^= (GEN_IS1_VR | GEN_IS1_DE);
 924                 *val = sc->vga_sts1;
 925                 break;
 926         case GEN_FEATURE_CTRL_PORT:
 927                 // OpenBSD calls this with bytes = 1
 928                 //assert(0);
 929                 *val = 0;
 930                 break;
 931         case 0x3c3:
 932                 *val = 0;
 933                 break;
 934         default:
 935                 printf("XXX vga_port_in_handler() unhandled port 0x%x\n", port);
 936                 //assert(0);
 937                 return (-1);
 938         }
 939
 940         return (0);
 941 }
 942
 943 static int
 944 vga_port_out_handler(struct vmctx *ctx, int in, int port, int bytes,
 945                      uint8_t val, void *arg)
 946 {
 947         struct vga_softc *sc = arg;
 948
 949         switch (port) {
 950         case CRTC_IDX_MONO_PORT:
 951         case CRTC_IDX_COLOR_PORT:
 952                 sc->vga_crtc.crtc_index = val;
 953                 break;
 954         case CRTC_DATA_MONO_PORT:
 955         case CRTC_DATA_COLOR_PORT:
 956                 switch (sc->vga_crtc.crtc_index) {
 957                 case CRTC_HORIZ_TOTAL:
 958                         sc->vga_crtc.crtc_horiz_total = val;
 959                         break;
 960                 case CRTC_HORIZ_DISP_END:
 961                         sc->vga_crtc.crtc_horiz_disp_end = val;
 962                         break;
 963                 case CRTC_START_HORIZ_BLANK:
 964                         sc->vga_crtc.crtc_start_horiz_blank = val;
 965                         break;
 966                 case CRTC_END_HORIZ_BLANK:
 967                         sc->vga_crtc.crtc_end_horiz_blank = val;
 968                         break;
 969                 case CRTC_START_HORIZ_RETRACE:
 970                         sc->vga_crtc.crtc_start_horiz_retrace = val;
 971                         break;
 972                 case CRTC_END_HORIZ_RETRACE:
 973                         sc->vga_crtc.crtc_end_horiz_retrace = val;
 974                         break;
 975                 case CRTC_VERT_TOTAL:
 976                         sc->vga_crtc.crtc_vert_total = val;
 977                         break;
 978                 case CRTC_OVERFLOW:
 979                         sc->vga_crtc.crtc_overflow = val;
 980                         break;
 981                 case CRTC_PRESET_ROW_SCAN:
 982                         sc->vga_crtc.crtc_present_row_scan = val;
 983                         break;
 984                 case CRTC_MAX_SCAN_LINE:
 985                         sc->vga_crtc.crtc_max_scan_line = val;
 986                         break;
 987                 case CRTC_CURSOR_START:
 988                         sc->vga_crtc.crtc_cursor_start = val;
 989                         sc->vga_crtc.crtc_cursor_on = (val & CRTC_CS_CO) == 0;
 990                         break;
 991                 case CRTC_CURSOR_END:
 992                         sc->vga_crtc.crtc_cursor_end = val;
 993                         break;
 994                 case CRTC_START_ADDR_HIGH:
 995                         sc->vga_crtc.crtc_start_addr_high = val;
 996                         sc->vga_crtc.crtc_start_addr &= 0x00ff;
 997                         sc->vga_crtc.crtc_start_addr |= (val << 8);
 998                         break;
 999                 case CRTC_START_ADDR_LOW:
1000                         sc->vga_crtc.crtc_start_addr_low = val;
1001                         sc->vga_crtc.crtc_start_addr &= 0xff00;
1002                         sc->vga_crtc.crtc_start_addr |= (val & 0xff);
1003                         break;
1004                 case CRTC_CURSOR_LOC_HIGH:
1005                         sc->vga_crtc.crtc_cursor_loc_high = val;
1006                         sc->vga_crtc.crtc_cursor_loc &= 0x00ff;
1007                         sc->vga_crtc.crtc_cursor_loc |= (val << 8);
1008                         break;
1009                 case CRTC_CURSOR_LOC_LOW:
1010                         sc->vga_crtc.crtc_cursor_loc_low = val;
1011                         sc->vga_crtc.crtc_cursor_loc &= 0xff00;
1012                         sc->vga_crtc.crtc_cursor_loc |= (val & 0xff);
1013                         break;
1014                 case CRTC_VERT_RETRACE_START:
1015                         sc->vga_crtc.crtc_vert_retrace_start = val;
1016                         break;
1017                 case CRTC_VERT_RETRACE_END:
1018                         sc->vga_crtc.crtc_vert_retrace_end = val;
1019                         break;
1020                 case CRTC_VERT_DISP_END:
1021                         sc->vga_crtc.crtc_vert_disp_end = val;
1022                         break;
1023                 case CRTC_OFFSET:
1024                         sc->vga_crtc.crtc_offset = val;
1025                         break;
1026                 case CRTC_UNDERLINE_LOC:
1027                         sc->vga_crtc.crtc_underline_loc = val;
1028                         break;
1029                 case CRTC_START_VERT_BLANK:
1030                         sc->vga_crtc.crtc_start_vert_blank = val;
1031                         break;
1032                 case CRTC_END_VERT_BLANK:
1033                         sc->vga_crtc.crtc_end_vert_blank = val;
1034                         break;
1035                 case CRTC_MODE_CONTROL:
1036                         sc->vga_crtc.crtc_mode_ctrl = val;
1037                         break;
1038                 case CRTC_LINE_COMPARE:
1039                         sc->vga_crtc.crtc_line_compare = val;
1040                         break;
1041                 default:
1042                         //printf("XXX VGA CRTC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_crtc.crtc_index);
1043                         assert(0);
1044                         break;
1045                 }
1046                 break;
1047         case ATC_IDX_PORT:
1048                 if (sc->vga_atc.atc_flipflop == 0) {
1049                         if (sc->vga_atc.atc_index & 0x20)
1050                                 assert(0);
1051                         sc->vga_atc.atc_index = val & ATC_IDX_MASK;
1052                 } else {
1053                         switch (sc->vga_atc.atc_index) {
1054                         case ATC_PALETTE0 ... ATC_PALETTE15:
1055                                 sc->vga_atc.atc_palette[sc->vga_atc.atc_index] = val & 0x3f;
1056                                 break;
1057                         case ATC_MODE_CONTROL:
1058                                 sc->vga_atc.atc_mode = val;
1059                                 break;
1060                         case ATC_OVERSCAN_COLOR:
1061                                 sc->vga_atc.atc_overscan_color = val;
1062                                 break;
1063                         case ATC_COLOR_PLANE_ENABLE:
1064                                 sc->vga_atc.atc_color_plane_enb = val;
1065                                 break;
1066                         case ATC_HORIZ_PIXEL_PANNING:
1067                                 sc->vga_atc.atc_horiz_pixel_panning = val;
1068                                 break;
1069                         case ATC_COLOR_SELECT:
1070                                 sc->vga_atc.atc_color_select = val;
1071                                 sc->vga_atc.atc_color_select_45 =
1072                                         (val & ATC_CS_C45) << 4;
1073                                 sc->vga_atc.atc_color_select_67 =
1074                                         ((val & ATC_CS_C67) >> 2) << 6;
1075                                 break;
1076                         default:
1077                                 //printf("XXX VGA ATC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_atc.atc_index);
1078                                 assert(0);
1079                                 break;
1080                         }
1081                 }
1082                 sc->vga_atc.atc_flipflop ^= 1;
1083                 break;
1084         case ATC_DATA_PORT:
1085                 break;
1086         case SEQ_IDX_PORT:
1087                 sc->vga_seq.seq_index = val & 0x1f;
1088                 break;
1089         case SEQ_DATA_PORT:
1090                 switch (sc->vga_seq.seq_index) {
1091                 case SEQ_RESET:
1092                         sc->vga_seq.seq_reset = val;
1093                         break;
1094                 case SEQ_CLOCKING_MODE:
1095                         sc->vga_seq.seq_clock_mode = val;
1096                         sc->vga_seq.seq_cm_dots = (val & SEQ_CM_89) ? 8 : 9;
1097                         break;
1098                 case SEQ_MAP_MASK:
1099                         sc->vga_seq.seq_map_mask = val;
1100                         break;
1101                 case SEQ_CHAR_MAP_SELECT:
1102                         sc->vga_seq.seq_cmap_sel = val;
1103
1104                         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;
1105                         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;
1106                         break;
1107                 case SEQ_MEMORY_MODE:
1108                         sc->vga_seq.seq_mm = val;
1109                         /* Windows queries Chain4 */
1110                         //assert((sc->vga_seq.seq_mm & SEQ_MM_C4) == 0);
1111                         break;
1112                 default:
1113                         //printf("XXX VGA SEQ: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_seq.seq_index);
1114                         assert(0);
1115                         break;
1116                 }
1117                 break;
1118         case DAC_MASK:
1119                 break;
1120         case DAC_IDX_RD_PORT:
1121                 sc->vga_dac.dac_rd_index = val;
1122                 sc->vga_dac.dac_rd_subindex = 0;
1123                 break;
1124         case DAC_IDX_WR_PORT:
1125                 sc->vga_dac.dac_wr_index = val;
1126                 sc->vga_dac.dac_wr_subindex = 0;
1127                 break;
1128         case DAC_DATA_PORT:
1129                 sc->vga_dac.dac_palette[3 * sc->vga_dac.dac_wr_index +
1130                                         sc->vga_dac.dac_wr_subindex] = val;
1131                 sc->vga_dac.dac_wr_subindex++;
1132                 if (sc->vga_dac.dac_wr_subindex == 3) {
1133                         sc->vga_dac.dac_palette_rgb[sc->vga_dac.dac_wr_index] =
1134                                 ((((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] << 2) |
1135                                    ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] & 0x1) << 1) |
1136                                    (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 0] & 0x1)) << 16) |
1137                                  (((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] << 2) |
1138                                    ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] & 0x1) << 1) |
1139                                    (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 1] & 0x1)) << 8) |
1140                                  (((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] << 2) |
1141                                    ((sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] & 0x1) << 1) |
1142                                    (sc->vga_dac.dac_palette[3*sc->vga_dac.dac_wr_index + 2] & 0x1)) << 0));
1143
1144                         sc->vga_dac.dac_wr_index++;
1145                         sc->vga_dac.dac_wr_subindex = 0;
1146                 }
1147                 break;
1148         case GC_IDX_PORT:
1149                 sc->vga_gc.gc_index = val;
1150                 break;
1151         case GC_DATA_PORT:
1152                 switch (sc->vga_gc.gc_index) {
1153                 case GC_SET_RESET:
1154                         sc->vga_gc.gc_set_reset = val;
1155                         break;
1156                 case GC_ENABLE_SET_RESET:
1157                         sc->vga_gc.gc_enb_set_reset = val;
1158                         break;
1159                 case GC_COLOR_COMPARE:
1160                         sc->vga_gc.gc_color_compare = val;
1161                         break;
1162                 case GC_DATA_ROTATE:
1163                         sc->vga_gc.gc_rotate = val;
1164                         sc->vga_gc.gc_op = (val >> 3) & 0x3;
1165                         break;
1166                 case GC_READ_MAP_SELECT:
1167                         sc->vga_gc.gc_read_map_sel = val;
1168                         break;
1169                 case GC_MODE:
1170                         sc->vga_gc.gc_mode = val;
1171                         sc->vga_gc.gc_mode_c4 = (val & GC_MODE_C4) != 0;
1172                         assert(!sc->vga_gc.gc_mode_c4);
1173                         sc->vga_gc.gc_mode_oe = (val & GC_MODE_OE) != 0;
1174                         sc->vga_gc.gc_mode_rm = (val >> 3) & 0x1;
1175                         sc->vga_gc.gc_mode_wm = val & 0x3;
1176
1177                         if (sc->gc_image)
1178                                 sc->gc_image->vgamode = 1;
1179                         break;
1180                 case GC_MISCELLANEOUS:
1181                         sc->vga_gc.gc_misc = val;
1182                         sc->vga_gc.gc_misc_gm = val & GC_MISC_GM;
1183                         sc->vga_gc.gc_misc_mm = (val & GC_MISC_MM) >>
1184                             GC_MISC_MM_SHIFT;
1185                         break;
1186                 case GC_COLOR_DONT_CARE:
1187                         sc->vga_gc.gc_color_dont_care = val;
1188                         break;
1189                 case GC_BIT_MASK:
1190                         sc->vga_gc.gc_bit_mask = val;
1191                         break;
1192                 default:
1193                         //printf("XXX VGA GC: outb 0x%04x, 0x%02x at index %d\n", port, val, sc->vga_gc.gc_index);
1194                         assert(0);
1195                         break;
1196                 }
1197                 break;
1198         case GEN_INPUT_STS0_PORT:
1199                 /* write to Miscellaneous Output Register */
1200                 sc->vga_misc = val;
1201                 break;
1202         case GEN_INPUT_STS1_MONO_PORT:
1203         case GEN_INPUT_STS1_COLOR_PORT:
1204                 /* write to Feature Control Register */
1205                 break;
1206 //      case 0x3c3:
1207 //              break;
1208         default:
1209                 printf("XXX vga_port_out_handler() unhandled port 0x%x, val 0x%x\n", port, val);
1210                 //assert(0);
1211                 return (-1);
1212         }
1213         return (0);
1214 }
1215
1216 static int
1217 vga_port_handler(struct vmctx *ctx, int vcpu, int in, int port, int bytes,
1218                  uint32_t *eax, void *arg)
1219 {
1220         uint8_t val;
1221         int error;
1222
1223         switch (bytes) {
1224         case 1:
1225                 if (in) {
1226                         *eax &= ~0xff;
1227                         error = vga_port_in_handler(ctx, in, port, 1,
1228                                                     &val, arg);
1229                         if (!error) {
1230                                 *eax |= val & 0xff;
1231                         }
1232                 } else {
1233                         val = *eax & 0xff;
1234                         error = vga_port_out_handler(ctx, in, port, 1,
1235                                                      val, arg);
1236                 }
1237                 break;
1238         case 2:
1239                 if (in) {
1240                         *eax &= ~0xffff;
1241                         error = vga_port_in_handler(ctx, in, port, 1,
1242                                                     &val, arg);
1243                         if (!error) {
1244                                 *eax |= val & 0xff;
1245                         }
1246                         error = vga_port_in_handler(ctx, in, port + 1, 1,
1247                                                     &val, arg);
1248                         if (!error) {
1249                                 *eax |= (val & 0xff) << 8;
1250                         }
1251                 } else {
1252                         val = *eax & 0xff;
1253                         error = vga_port_out_handler(ctx, in, port, 1,
1254                                                      val, arg);
1255                         val = (*eax >> 8) & 0xff;
1256                         error =vga_port_out_handler(ctx, in, port + 1, 1,
1257                                                     val, arg);
1258                 }
1259                 break;
1260         default:
1261                 assert(0);
1262                 return (-1);
1263         }
1264
1265         return (error);
1266 }
1267
1268 void *
1269 vga_init(int io_only)
1270 {
1271         struct inout_port iop;
1272         struct vga_softc *sc;
1273         int port, error;
1274
1275         sc = calloc(1, sizeof(struct vga_softc));
1276
1277         bzero(&iop, sizeof(struct inout_port));
1278         iop.name = "VGA";
1279         for (port = VGA_IOPORT_START; port <= VGA_IOPORT_END; port++) {
1280                 iop.port = port;
1281                 iop.size = 1;
1282                 iop.flags = IOPORT_F_INOUT;
1283                 iop.handler = vga_port_handler;
1284                 iop.arg = sc;
1285
1286                 error = register_inout(&iop);
1287                 assert(error == 0);
1288         }
1289
1290         sc->gc_image = console_get_image();
1291
1292         /* only handle io ports; vga graphics is disabled */
1293         if (io_only)
1294                 return(sc);
1295
1296         sc->mr.name = "VGA memory";
1297         sc->mr.flags = MEM_F_RW;
1298         sc->mr.base = 640 * KB;
1299         sc->mr.size = 128 * KB;
1300         sc->mr.handler = vga_mem_handler;
1301         sc->mr.arg1 = sc;
1302         error = register_mem_fallback(&sc->mr);
1303         assert(error == 0);
1304
1305         sc->vga_ram = malloc(256 * KB);
1306         memset(sc->vga_ram, 0, 256 * KB);
1307
1308         {
1309                 static uint8_t palette[] = {
1310                         0x00,0x00,0x00, 0x00,0x00,0x2a, 0x00,0x2a,0x00, 0x00,0x2a,0x2a,
1311                         0x2a,0x00,0x00, 0x2a,0x00,0x2a, 0x2a,0x2a,0x00, 0x2a,0x2a,0x2a,
1312                         0x00,0x00,0x15, 0x00,0x00,0x3f, 0x00,0x2a,0x15, 0x00,0x2a,0x3f,
1313                         0x2a,0x00,0x15, 0x2a,0x00,0x3f, 0x2a,0x2a,0x15, 0x2a,0x2a,0x3f,
1314                 };
1315                 int i;
1316
1317                 memcpy(sc->vga_dac.dac_palette, palette, 16 * 3 * sizeof (uint8_t));
1318                 for (i = 0; i < 16; i++) {
1319                         sc->vga_dac.dac_palette_rgb[i] =
1320                                 ((((sc->vga_dac.dac_palette[3*i + 0] << 2) |
1321                                    ((sc->vga_dac.dac_palette[3*i + 0] & 0x1) << 1) |
1322                                    (sc->vga_dac.dac_palette[3*i + 0] & 0x1)) << 16) |
1323                                  (((sc->vga_dac.dac_palette[3*i + 1] << 2) |
1324                                    ((sc->vga_dac.dac_palette[3*i + 1] & 0x1) << 1) |
1325                                    (sc->vga_dac.dac_palette[3*i + 1] & 0x1)) << 8) |
1326                                  (((sc->vga_dac.dac_palette[3*i + 2] << 2) |
1327                                    ((sc->vga_dac.dac_palette[3*i + 2] & 0x1) << 1) |
1328                                    (sc->vga_dac.dac_palette[3*i + 2] & 0x1)) << 0));
1329                 }
1330         }
1331
1332         return (sc);
1333 }