usr.sbin/bhyve/vga.c

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