palacios/src/devices/vga.c

   1 /*
   2  * This file is part of the Palacios Virtual Machine Monitor developed
   3  * by the V3VEE Project with funding from the United States National
   4  * Science Foundation and the Department of Energy.
   5  *
   6  * The V3VEE Project is a joint project between Northwestern University
   7  * and the University of New Mexico.  You can find out more at
   8  * http://www.v3vee.org
   9  *
  10  * Copyright (c) 2011, Peter Dinda <pdinda@northwestern.edu>
  11  * Copyright (c) 2011, The V3VEE Project <http://www.v3vee.org>
  12  * All rights reserved.
  13  *
  14  * Author: Peter Dinda <pdinda@northwestern.edu>
  15  *
  16  * This is free software.  You are permitted to use,
  17  * redistribute, and modify it as specified in the file "V3VEE_LICENSE".
  18  */
  19
  20 #include <palacios/vmm.h>
  21 #include <palacios/vmm_dev_mgr.h>
  22 #include <palacios/vmm_types.h>
  23 #include <palacios/vm_guest_mem.h>
  24 #include <palacios/vmm_io.h>
  25 #include <interfaces/vmm_graphics_console.h>
  26
  27 #include "vga_regs.h"
  28
  29 #ifndef V3_CONFIG_DEBUG_VGA
  30 #undef PrintDebug
  31 #define PrintDebug(fmt, args...)
  32 #endif
  33
  34 #define DEBUG_MEM_DATA    0
  35 #define DEBUG_DEEP_MEM    0
  36 #define DEBUG_DEEP_RENDER 0
  37
  38
  39 #define MEM_REGION_START 0xa0000
  40 #define MEM_REGION_END   0xc0000
  41 #define MEM_REGION_NUM_PAGES (((MEM_REGION_END)-(MEM_REGION_START))/4096)
  42
  43 #define MAP_SIZE 65536
  44 #define MAP_NUM  4
  45
  46 #define UPDATES_PER_RENDER 100
  47
  48 typedef uint8_t *vga_map; // points to MAP_SIZE data
  49
  50 #define VGA_MAXX 1024
  51 #define VGA_MAXY 768
  52
  53
  54 #define VGA_MISC_OUT_READ 0x3cc
  55 #define VGA_MISC_OUT_WRITE 0x3c2
  56
  57 #define VGA_INPUT_STAT0_READ 0x3c2
  58
  59 #define VGA_INPUT_STAT1_READ_MONO 0x3ba
  60 #define VGA_INPUT_STAT1_READ_COLOR 0x3da
  61
  62 #define VGA_FEATURE_CONTROL_READ 0x3ca
  63 #define VGA_FEATURE_CONTROL_WRITE_MONO 0x3ba
  64 #define VGA_FEATURE_CONTROL_WRITE_COLOR 0x3da
  65
  66 #define VGA_VIDEO_SUBSYS_ENABLE 0x3c3
  67
  68 #define VGA_SEQUENCER_ADDRESS 0x3c4
  69 #define VGA_SEQUENCER_DATA 0x3c5
  70 #define VGA_SEQUENCER_NUM 5
  71
  72
  73 #define VGA_CRT_CONTROLLER_ADDRESS_MONO 0x3b4
  74 #define VGA_CRT_CONTROLLER_ADDRESS_COLOR 0x3d4
  75 #define VGA_CRT_CONTROLLER_DATA_MONO 0x3b5
  76 #define VGA_CRT_CONTROLLER_DATA_COLOR 0x3d5
  77 #define VGA_CRT_CONTROLLER_NUM 25
  78
  79
  80 #define VGA_GRAPHICS_CONTROLLER_ADDRESS 0x3ce
  81 #define VGA_GRAPHICS_CONTROLLER_DATA 0x3cf
  82 #define VGA_GRAPHICS_CONTROLLER_NUM 9
  83
  84 #define VGA_ATTRIBUTE_CONTROLLER_ADDRESS_AND_WRITE 0x3c0
  85 #define VGA_ATTRIBUTE_CONTROLLER_READ 0x3c1
  86 #define VGA_ATTRIBUTE_CONTROLLER_NUM 21
  87
  88 #define VGA_DAC_WRITE_ADDR 0x3c8
  89 #define VGA_DAC_READ_ADDR 0x3c7
  90 #define VGA_DAC_DATA 0x3c9
  91 #define VGA_DAC_PIXEL_MASK 0x3c6
  92
  93 #define VGA_DAC_NUM_ENTRIES 256
  94
  95
  96 #define VGA_FONT_WIDTH      8
  97 #define VGA_MAX_FONT_HEIGHT 32
  98
  99 struct vga_misc_regs {
 100     /* Read: 0x3cc; Write: 0x3c2 */
 101     struct vga_misc_out_reg        vga_misc_out;
 102     /* Read: 0x3c2 */
 103     struct vga_input_stat0_reg     vga_input_stat0;
 104     /* Read: 0x3?a  3ba for mono; 3da for cga set by misc.io_addr_sel */
 105     struct vga_input_stat1_reg     vga_input_stat1;
 106     /* Read: 0x3ca; Write: 0x3?a 3ba for mono 3da for color - set by misc.io_addr_sel*/
 107     struct vga_feature_control_reg vga_feature_control;
 108     /* Read: 0x3c3; Write: 0x3c3 */
 109     struct vga_video_subsys_enable_reg vga_video_subsys_enable;
 110 } __attribute__((packed));
 111
 112 struct vga_sequencer_regs {
 113     /*   Address register is 0x3c4, data register is 0x3c5 */
 114     /* 0x3c4 */
 115     struct vga_sequencer_addr_reg vga_sequencer_addr;
 116
 117     /* these can be accessed via the index, offset on start
 118        or via the specific regs.   For this reason, it is essential
 119        that this is all packed and that the order does not change */
 120
 121     uint8_t  vga_sequencer_regs[0];
 122
 123     /* Index 0 */
 124     struct vga_reset_reg   vga_reset;
 125     /* Index 1 */
 126     struct vga_clocking_mode_reg vga_clocking_mode;
 127     /* Index 2 */
 128     struct vga_map_mask_reg vga_map_mask;
 129     /* Index 3 */
 130     struct vga_char_map_select_reg vga_char_map_select;
 131     /* Index 4 */
 132     struct vga_mem_mode_reg  vga_mem_mode;
 133 } __attribute__((packed));
 134
 135 struct vga_crt_controller_regs {
 136     /* Address Register is 0x3b4 or 0x3d4 */
 137     /* Data register is 0x3b5 or 0x3d5 based on mono versus color */
 138     struct vga_crt_addr_reg vga_crt_addr;
 139
 140     /* these can be accessed via the index, offset on start
 141        or via the specific regs.   For this reason, it is essential
 142        that this is all packed and that the order does not change */
 143
 144     uint8_t  vga_crt_controller_regs[0];
 145
 146     /* index 0 */
 147     vga_horizontal_total_reg vga_horizontal_total;
 148     /* index 1 */
 149     vga_horizontal_display_enable_end_reg vga_horizontal_display_enable_end;
 150     /* index 2 */
 151     vga_start_horizontal_blanking_reg vga_start_horizontal_blanking;
 152     /* index 3 */
 153     struct vga_end_horizontal_blanking_reg vga_end_horizontal_blanking;
 154     /* index 4 */
 155     vga_start_horizontal_retrace_pulse_reg vga_start_horizontal_retrace_pulse;
 156     /* index 5 */
 157     struct vga_end_horizontal_retrace_reg vga_end_horizontal_retrace;
 158     /* index 6 */
 159     vga_vertical_total_reg vga_vertical_total;
 160     /* index 7 */
 161     struct vga_overflow_reg vga_overflow;
 162     /* index 8 */
 163     struct vga_preset_row_scan_reg vga_preset_row_scan;
 164     /* index 9 */
 165     struct vga_max_row_scan_reg vga_max_row_scan;
 166     /* index 10 */
 167     struct vga_cursor_start_reg vga_cursor_start;
 168     /* index 11 */
 169     struct vga_cursor_end_reg vga_cursor_end;
 170     /* index 12 */
 171     vga_start_address_high_reg vga_start_address_high;
 172     /* index 13 */
 173     vga_start_address_low_reg vga_start_address_low;
 174     /* index 14 */
 175     vga_cursor_location_high_reg vga_cursor_location_high;
 176     /* index 15 */
 177     vga_cursor_location_low_reg vga_cursor_location_low;
 178     /* index 16 */
 179     vga_vertical_retrace_start_reg vga_vertical_retrace_start;
 180     /* index 17 */
 181     struct vga_vertical_retrace_end_reg vga_vertical_retrace_end;
 182     /* index 18 */
 183     vga_vertical_display_enable_end_reg vga_vertical_display_enable_end;
 184     /* index 19 */
 185     vga_offset_reg vga_offset;
 186     /* index 20 */
 187     struct vga_underline_location_reg vga_underline_location;
 188     /* index 21 */
 189     vga_start_vertical_blanking_reg vga_start_vertical_blanking;
 190     /* index 22 */
 191     vga_end_vertical_blanking_reg vga_end_vertical_blanking;
 192     /* index 23 */
 193     struct vga_crt_mode_control_reg vga_crt_mode_control;
 194     /* index 24 */
 195     vga_line_compare_reg vga_line_compare;
 196 } __attribute__((packed));
 197
 198 struct vga_graphics_controller_regs {
 199     /*   Address: 0x3ce    Data: 0x3cf */
 200
 201     /* 0x3ce */
 202     struct vga_graphics_ctrl_addr_reg vga_graphics_ctrl_addr;
 203
 204     /* these can be accessed via the index, offset on start
 205        or via the specific regs.   For this reason, it is essential
 206        that this is all packed and that the order does not change */
 207
 208     uint8_t  vga_graphics_controller_regs[0];
 209
 210     /* Index 0 */
 211     struct vga_set_reset_reg vga_set_reset;
 212     /* Index 1 */
 213     struct vga_enable_set_reset_reg vga_enable_set_reset;
 214     /* Index 2 */
 215     struct vga_color_compare_reg vga_color_compare;
 216     /* Index 3 */
 217     struct vga_data_rotate_reg vga_data_rotate;
 218     /* Index 4 */
 219     struct vga_read_map_select_reg vga_read_map_select;
 220     /* Index 5 */
 221     struct vga_graphics_mode_reg vga_graphics_mode;
 222     /* Index 6 */
 223     struct vga_misc_reg vga_misc;
 224     /* Index 7 */
 225     struct vga_color_dont_care_reg vga_color_dont_care;
 226     /* Index 8 */
 227     vga_bit_mask_reg vga_bit_mask;
 228 } __attribute__((packed));
 229
 230
 231 struct vga_attribute_contoller_regs {
 232     /*
 233       Address AND WRITE: 0x3c0
 234       Read: 0x3c1
 235
 236       The write protocol is to write the index to 0x3c0 followed by
 237       the data.  The read protocol is to write the index to 0x3c0
 238       and then read from 0x3c1
 239
 240       IMPORTANT: write address, write data flips state back to write address
 241       write address, read data DOES NOT
 242
 243       To reset to write address state, read input status register 1
 244 */
 245     enum { ATTR_ADDR, ATTR_DATA }  state;  //state of the flip flop
 246
 247     /* 0x3c0 */
 248     struct vga_attribute_controller_address_reg vga_attribute_controller_addr;
 249
 250
 251
 252     /* these can be accessed via the index, offset on start
 253        or via the specific regs.   For this reason, it is essential
 254        that this is all packed and that the order does not change */
 255
 256     uint8_t  vga_attribute_controller_regs[0];
 257
 258     /* Indices 0..15 */
 259     vga_internal_palette_regs   vga_internal_palette;
 260     /* Index 16 */
 261     struct vga_attribute_mode_control_reg vga_attribute_mode_control;
 262     /* Index 17 */
 263     vga_overscan_color_reg vga_overscan_color;
 264     /* Index 18 */
 265     struct vga_color_plane_enable_reg vga_color_plane_enable;
 266     /* Index 19 */
 267     struct vga_horizontal_pixel_pan_reg vga_horizontal_pixel_pan;
 268     /* Index 20 */
 269     struct vga_color_select_reg vga_color_select;
 270 } __attribute__((packed));
 271
 272 struct vga_dac_regs {
 273     enum {DAC_READ=0, DAC_WRITE} state;
 274     enum {RED=0,GREEN,BLUE} channel;
 275     vga_dac_pixel_mask_reg vga_pixel_mask;
 276     vga_dac_write_addr_reg vga_dac_write_addr;
 277     vga_dac_read_addr_reg vga_dac_read_addr;
 278     // the dac_data register is used only to access the registers
 279     // and thus has no representation here
 280     vga_palette_reg vga_dac_palette[VGA_DAC_NUM_ENTRIES];
 281 } __attribute__((packed));
 282
 283
 284 struct vga_internal {
 285     struct vm_device *dev;
 286
 287     bool passthrough;
 288     bool skip_next_passthrough_out; // for word access
 289
 290     struct v3_frame_buffer_spec  target_spec;
 291     v3_graphics_console_t host_cons;
 292
 293     uint32_t updates_since_render;
 294
 295     struct frame_buf *framebuf; // we render to this
 296
 297     //    void *mem_store;     // This is the region where the memory hooks will go
 298
 299     vga_map  map[MAP_NUM];  // the maps that the host writes to
 300
 301     uint8_t  latch[MAP_NUM];  // written to in any read, used during writes
 302
 303     /* Range of I/O ports here for backward compat with MDA and CGA */
 304     struct vga_misc_regs  vga_misc;
 305
 306     /* Address Register is 0x3b4 or 0x3d4 */
 307     /* Data register is 0x3b5 or 0x3d5 based on MDA/CGA/VGA (backward compat) */
 308     struct vga_crt_controller_regs vga_crt_controller;
 309
 310     /*   Address register is 0x3c4, data register is 0x3c5 */
 311     struct vga_sequencer_regs vga_sequencer;
 312
 313     /*   Address: 0x3ce    Data: 0x3cf */
 314     struct vga_graphics_controller_regs vga_graphics_controller;
 315
 316     /*
 317       Address AND WRITE: 0x3c0
 318       Read: 0x3c1
 319       Flip-Flop
 320     */
 321     struct vga_attribute_contoller_regs vga_attribute_controller;
 322
 323     /*
 324       address for reads: 0x3c7 (also resets state machine for access to 18 bit regs
 325       address for writes: 0x3c8 ("")
 326       data: 0x3c9
 327       pixel mask: 0x3c6 - do not write (init to 0xff)
 328     */
 329     struct vga_dac_regs vga_dac;
 330 };
 331
 332
 333 typedef enum {PLANAR_SHIFT, PACKED_SHIFT, C256_SHIFT} shift_mode_t;
 334
 335
 336 static void find_text_char_dim(struct vga_internal *vga, uint32_t *w, uint32_t *h)
 337 {
 338     *w = (vga->vga_sequencer.vga_clocking_mode.dot8 ? 8 : 9);
 339
 340     *h = vga->vga_crt_controller.vga_max_row_scan.max_scan_line+1;
 341
 342 }
 343
 344 static void find_text_res(struct vga_internal *vga, uint32_t *width, uint32_t *height)
 345 {
 346     uint32_t vert_lsb, vert_msb;
 347     uint32_t ph;
 348     uint32_t ch, cw;
 349
 350     *width = (vga->vga_crt_controller.vga_horizontal_display_enable_end + 1)
 351         - vga->vga_crt_controller.vga_end_horizontal_blanking.display_enable_skew;
 352
 353     vert_lsb = vga->vga_crt_controller.vga_vertical_display_enable_end; // 8 bits here
 354     vert_msb = (vga->vga_crt_controller.vga_overflow.vertical_disp_enable_end9 << 1)  // 2 bits here
 355         + (vga->vga_crt_controller.vga_overflow.vertical_disp_enable_end8);
 356
 357     ph  = ( (vert_msb << 8) + vert_lsb + 1) ; // pixels high (scanlines)
 358
 359     find_text_char_dim(vga,&cw, &ch);
 360
 361     *height = ph / ch;
 362
 363 }
 364
 365
 366 static void find_text_data_start(struct vga_internal *vga, void **data)
 367 {
 368     uint32_t offset;
 369
 370     offset = vga->vga_crt_controller.vga_start_address_high;
 371     offset <<= 8;
 372     offset += vga->vga_crt_controller.vga_start_address_low;
 373
 374     *data = vga->map[0]+offset;
 375
 376 }
 377
 378
 379 static void find_text_attr_start(struct vga_internal *vga, void **data)
 380 {
 381     uint32_t offset;
 382
 383     offset = vga->vga_crt_controller.vga_start_address_high;
 384     offset <<= 8;
 385     offset += vga->vga_crt_controller.vga_start_address_low;
 386
 387     *data = vga->map[1]+offset;
 388
 389 }
 390
 391 static void find_text_cursor_pos(struct vga_internal *vga, uint32_t *x, uint32_t *y, void **data)
 392 {
 393     uint32_t w,h;
 394     uint32_t offset;
 395     uint32_t charsin;
 396     void *buf;
 397
 398     find_text_res(vga,&w,&h);
 399
 400     find_text_data_start(vga,&buf);
 401
 402     offset = vga->vga_crt_controller.vga_cursor_location_high;
 403     offset <<= 8;
 404     offset += vga->vga_crt_controller.vga_cursor_location_low;
 405
 406     *data = vga->map[0]+offset;
 407
 408     charsin = (uint32_t)(*data - buf);
 409
 410     *x = charsin % w;
 411     *y = charsin / w;
 412
 413 }
 414
 415
 416 static void find_text_font_start(struct vga_internal *vga, void **data, uint8_t char_map)
 417 {
 418     uint32_t mapa_offset, mapb_offset;
 419
 420
 421     switch (char_map) {
 422         case 0:
 423             mapa_offset = (vga->vga_sequencer.vga_char_map_select.char_map_a_sel_lsb << 1)
 424                            + vga->vga_sequencer.vga_char_map_select.char_map_a_sel_msb ;
 425             *data = vga->map[2] + mapa_offset;
 426             break;
 427
 428         case 1:
 429             mapb_offset = (vga->vga_sequencer.vga_char_map_select.char_map_b_sel_lsb << 1)
 430                            + vga->vga_sequencer.vga_char_map_select.char_map_b_sel_msb ;
 431             *data = vga->map[2] + mapb_offset;
 432             break;
 433         default:
 434             PrintError("vga: unknown char_map given to find_text_font_start\n");
 435             break;
 436
 437     }
 438 }
 439
 440 static int extended_fontset(struct vga_internal *vga)
 441 {
 442     if (vga->vga_sequencer.vga_mem_mode.extended_memory &&
 443         ! ( (vga->vga_sequencer.vga_char_map_select.char_map_a_sel_lsb
 444              == vga->vga_sequencer.vga_char_map_select.char_map_b_sel_lsb) &&
 445             (vga->vga_sequencer.vga_char_map_select.char_map_a_sel_msb
 446              == vga->vga_sequencer.vga_char_map_select.char_map_b_sel_msb))) {
 447         return 1;
 448     } else {
 449         return 0;
 450     }
 451
 452 }
 453
 454 static int blinking(struct vga_internal *vga)
 455 {
 456     return vga->vga_attribute_controller.vga_attribute_mode_control.enable_blink;
 457 }
 458
 459
 460 static void find_graphics_data_starting_offset(struct vga_internal *vga, uint32_t *offset)
 461 {
 462
 463     *offset = vga->vga_crt_controller.vga_start_address_high;
 464     *offset <<= 8;
 465     *offset += vga->vga_crt_controller.vga_start_address_low;
 466 }
 467
 468
 469 static void find_shift_mode(struct vga_internal *vga, shift_mode_t *mode)
 470 {
 471     if (vga->vga_graphics_controller.vga_graphics_mode.c256) {
 472         *mode=C256_SHIFT;
 473     } else {
 474         if (vga->vga_graphics_controller.vga_graphics_mode.shift_reg_mode) {
 475             *mode=PACKED_SHIFT;
 476         } else {
 477             *mode=PLANAR_SHIFT;
 478         }
 479     }
 480 }
 481
 482
 483 static void find_graphics_res(struct vga_internal *vga, uint32_t *width, uint32_t *height)
 484 {
 485     uint32_t vert_lsb, vert_msb;
 486
 487     *width = ((vga->vga_crt_controller.vga_horizontal_display_enable_end + 1)
 488               - vga->vga_crt_controller.vga_end_horizontal_blanking.display_enable_skew);
 489
 490     *width *= (vga->vga_sequencer.vga_clocking_mode.dot8 ? 8 : 9);
 491
 492     vert_lsb = vga->vga_crt_controller.vga_vertical_display_enable_end; // 8 bits here
 493     vert_msb = (vga->vga_crt_controller.vga_overflow.vertical_disp_enable_end9 << 1)  // 2 bits here
 494         + (vga->vga_crt_controller.vga_overflow.vertical_disp_enable_end8);
 495
 496     *height  = ( (vert_msb << 8) + vert_lsb + 1) ; // pixels high (scanlines)
 497
 498     // At this point we have the resolution in dot clocks across and scanlines top-to-bottom
 499     // This is usually the resolution in pixels, but it can be monkeyed with
 500     // at least in the following ways
 501
 502     // vga sequencer dot clock divide by two
 503     if (vga->vga_sequencer.vga_clocking_mode.dot_clock) {
 504         *width/=2;
 505         *height/=2;
 506     }
 507
 508     // crt_controller.max_row_scan.double_scan => each row twice for 200=>400
 509     if (vga->vga_crt_controller.vga_max_row_scan.double_scan) {
 510         *height/=2;
 511     }
 512
 513     // crt_controller.crt_mode_control.count_by_two => pixels twice as wide as normal
 514     if (vga->vga_crt_controller.vga_crt_mode_control.count_by_two) {
 515         *width /= 2;
 516     }
 517
 518     // crt_controller.crt_mode_control.horizontal_retrace_select => pixels twice as tall as normal
 519     if (vga->vga_crt_controller.vga_crt_mode_control.horizontal_retrace_select) {
 520         *height /= 2;
 521     }
 522
 523 }
 524
 525
 526 static void find_graphics_cursor_pos(struct vga_internal *vga, uint32_t *x, uint32_t *y)
 527 {
 528     // todo
 529     *x=*y=0;
 530 }
 531
 532
 533 static void dac_lookup_24bit_color(struct vga_internal *vga,
 534                                    uint8_t entry,
 535                                    uint8_t *red,
 536                                    uint8_t *green,
 537                                    uint8_t *blue)
 538 {
 539     // use internal or external palette?
 540
 541     vga_palette_reg *r = &(vga->vga_dac.vga_dac_palette[entry]);
 542
 543     // converting from 6 bits to 8 bits so << 2
 544     *red = (*r & 0x3f) << 2;
 545     *green = ((*r >> 8) & 0x3f) << 2;
 546     *blue = ((*r >> 16) & 0x3f) << 2;
 547
 548 }
 549
 550
 551 /*
 552   Colors work like this:
 553
 554   4 bit modes:   index is to the internal palette on the attribute controller
 555                  that supplies 6 bits, but we need 8 to index the dac
 556                  2 more (the msbs) are supplied from the color select register
 557                  we can optionally overwrite bits 5 and 4 from the color
 558                  select register as well, depending on a selection bit
 559                  in the mode control register.   The result of all this is
 560                  8 bit index for the dac
 561
 562   8 bit modes:   the attribute controller passes the index straight through
 563                  to the DAC.
 564
 565
 566   The DAC translates from the 8 bit index into 6 bits per color channel
 567   (18 bit color).   We mulitply by 4 to get 24 bit color.
 568 */
 569
 570 static void find_24bit_color(struct vga_internal *vga,
 571                              uint8_t val,
 572                              uint8_t *red,
 573                              uint8_t *green,
 574                              uint8_t *blue)
 575 {
 576     uint8_t di;  // ultimate dac index
 577
 578     if (vga->vga_attribute_controller.vga_attribute_mode_control.pixel_width) {
 579         // 8 bit mode does right to the DAC
 580         di=val;
 581     } else {
 582         struct vga_internal_palette_reg pr = vga->vga_attribute_controller.vga_internal_palette[val%16];
 583         di = pr.palette_data;
 584
 585         // Fix bits 5-4 if needed
 586         if (vga->vga_attribute_controller.vga_attribute_mode_control.p54_select) {
 587             di &= ~0x30;  // clear 5-4
 588             di |= vga->vga_attribute_controller.vga_color_select.sc4 << 4;
 589             di |= vga->vga_attribute_controller.vga_color_select.sc5 << 5;
 590         }
 591
 592         // We must always produce bits 6 and 7
 593         di &= ~0xc0; // clear 7-6
 594         di |= vga->vga_attribute_controller.vga_color_select.sc6 << 6;
 595         di |= vga->vga_attribute_controller.vga_color_select.sc7 << 7;
 596     }
 597
 598     dac_lookup_24bit_color(vga,di,red,green,blue);
 599 }
 600
 601 static void render_graphics(struct vga_internal *vga, void *fb)
 602 {
 603
 604     struct v3_frame_buffer_spec *spec = &(vga->target_spec);
 605
 606     uint32_t gw, gh; // graphics w/h
 607     uint32_t fw, fh; // fb w/h
 608     uint32_t rgw, rgh;  // region we can actually show on the frame buffer
 609
 610
 611     uint32_t fx, fy;     // pixel position within the frame buffer
 612
 613     uint32_t offset;     // offset into the maps
 614     uint8_t  m;        // map
 615     uint8_t  p;          // pixel in the current map byte  (0..7)
 616
 617     uint8_t r,g,b;  // looked up colors for entry
 618
 619     void    *pixel;   // current pixel in the fb
 620     uint8_t *red;     // and the channels in the pixel
 621     uint8_t *green;   //
 622     uint8_t *blue;    //
 623
 624     uint8_t db[4]; // 4 bytes read at a time
 625     uint8_t pb[8]; // 8 pixels assembled at a time
 626
 627     shift_mode_t sm;   // shift mode
 628
 629     uint32_t cur_x, cur_y;
 630
 631
 632     find_graphics_res(vga,&gw,&gh);
 633
 634     find_shift_mode(vga,&sm);
 635
 636     find_graphics_cursor_pos(vga,&cur_x,&cur_y);
 637
 638     find_graphics_data_starting_offset(vga,&offset);
 639
 640     fw = spec->width;
 641     fh = spec->height;
 642
 643
 644     PrintDebug("vga: attempting graphics render (%s): graphics_res=(%u,%u), fb_res=(%u,%u), "
 645                "fb=0x%p offset=0x%x\n",
 646                sm == PLANAR_SHIFT ? "planar shift" :
 647                sm == PACKED_SHIFT ? "packed shift" :
 648                sm == C256_SHIFT ? "color256 shift" : "UNKNOWN",
 649                gw,gh,fw,fh,fb,offset);
 650
 651     // First we need to clip to what we can actually show
 652     rgw = gw < fw ? gw : fw;
 653     rgh = gh < fh ? gh : fh;
 654
 655     if (gw%8) {
 656         PrintError("vga: warning: graphics width is not a multiple of 8\n");
 657     }
 658
 659
 660
 661     // Now we scan across by row
 662     for (fy=0;fy<gh;fy++) {
 663         // by column
 664         for (fx=0;fx<gw;
 665              fx += (sm==C256_SHIFT ? 4 : 8) , offset++ ) {
 666
 667             // if any of these pixels are in the rendger region
 668             if (fy < rgh && fx < rgw) {
 669                 // assemble all 4 or 8 pixels
 670
 671                 // fetch the data bytes
 672                 for (m=0;m<4;m++) {
 673                     db[m]=*((uint8_t*)(vga->map[m]+offset));
 674                 }
 675
 676                 // assemble
 677                 switch (sm) {
 678                     case PLANAR_SHIFT:
 679                         for (p=0;p<8;p++) {
 680                             pb[p]=
 681                                 (( db[0] >> 7) & 0x1) |
 682                                 (( db[1] >> 6) & 0x2) |
 683                                 (( db[2] >> 5) & 0x4) |
 684                                 (( db[3] >> 4) & 0x8) ;
 685
 686                             for (m=0;m<4;m++) {
 687                                 db[m] <<= 1;
 688                             }
 689                         }
 690                         break;
 691
 692                     case PACKED_SHIFT:
 693                         // first 4 pixels use planes 0 and 2
 694                         for (p=0;p<4;p++) {
 695                             pb[p] =
 696                                 ((db[2] >> 4) & 0xc) |
 697                                 ((db[0] >> 6) & 0x3) ;
 698                             db[2] <<= 2;
 699                             db[0] <<= 2;
 700                         }
 701                         break;
 702
 703                         // next 4 pixels use planes 1 and 3
 704                         for (p=4;p<8;p++) {
 705                             pb[p] =
 706                                 ((db[3] >> 4) & 0xc) |
 707                                 ((db[1] >> 6) & 0x3) ;
 708                             db[3] <<= 2;
 709                             db[1] <<= 2;
 710                         }
 711                         break;
 712
 713                     case C256_SHIFT:
 714                         // this one is either very bizarre or as simple as this
 715                         for (p=0;p<4;p++) {
 716                             pb[p] = db[p];
 717                         }
 718                         break;
 719                 }
 720
 721                 // draw each pixel
 722                 for (p=0;p< (sm==C256_SHIFT ? 4 : 8);p++) {
 723
 724                     // find its color
 725                     find_24bit_color(vga,pb[p],&r,&g,&b);
 726
 727                     // find its position in the framebuffer;
 728                     pixel =  fb + (((fx + p) + (fy*spec->width)) * spec->bytes_per_pixel);
 729                     red = pixel + spec->red_offset;
 730                     green = pixel + spec->green_offset;
 731                     blue = pixel + spec->blue_offset;
 732
 733                     // draw it
 734                     *red=r;
 735                     *green=g;
 736                     *blue=b;
 737                 }
 738             }
 739         }
 740     }
 741
 742     PrintDebug("vga: render done\n");
 743 }
 744
 745
 746 static void render_text_cursor(struct vga_internal *vga, void *fb)
 747 {
 748 }
 749
 750
 751
 752
 753 //
 754 // A variant of this function could render to
 755 // a text console interface as well
 756 //
 757 static void render_text(struct vga_internal *vga, void *fb)
 758 {
 759     // line graphics enable bit means to  dupe column 8 to 9 when
 760     // in 9 dot wide mode
 761     // otherwise 9th dot is background
 762
 763     struct v3_frame_buffer_spec *spec = &(vga->target_spec);
 764
 765     uint32_t gw, gh; // graphics w/h
 766     uint32_t tw, th; // text w/h
 767     uint32_t rtw, rth; // rendered text w/h
 768     uint32_t cw, ch; // char font w/h including 8/9
 769     uint32_t fw, fh; // fb w/h
 770
 771     uint32_t px, py; // cursor position
 772
 773     uint32_t x, y, l, p; // text location, line and pixel within the char
 774     uint32_t fx, fy;     // pixel position within the frame buffer
 775
 776     uint8_t *text_start;
 777     uint8_t *text;    // points to current char
 778     uint8_t *attr;    // and its matching attribute
 779     uint8_t *curs;    // to where the cursor is
 780     uint8_t *font;    // to where the current font is
 781
 782     uint8_t fg_entry; // foreground color entry
 783     uint8_t bg_entry; // background color entry
 784     uint8_t fgr,fgg,fgb;    // looked up  foreground colors
 785     uint8_t bgr,bgg,bgb;    // looked up  bg colors
 786
 787     uint8_t ct, ca;   // the current char and attribute
 788     struct vga_attribute_byte a; // decoded attribute
 789
 790     void *pixel;      // current pixel in the fb
 791     uint8_t *red;     // and the channels in the pixel
 792     uint8_t *green;   //
 793     uint8_t *blue;    //
 794
 795
 796
 797     find_graphics_res(vga,&gw,&gh);
 798     find_text_res(vga,&tw,&th);
 799     find_text_char_dim(vga,&cw,&ch);
 800     fw = spec->width;
 801     fh = spec->height;
 802
 803     find_text_cursor_pos(vga,&px,&py,(void**)&curs);
 804     find_text_data_start(vga,(void**)&text);
 805     find_text_attr_start(vga,(void**)&attr);
 806
 807     find_text_font_start(vga,(void**)&font,0); // will need to switch this as we go since it is part of attr
 808
 809     PrintDebug("vga: attempting text render: graphics_res=(%u,%u), fb_res=(%u,%u), text_res=(%u,%u), "
 810                "char_res=(%u,%u), cursor=(%u,%u) font=0x%p, text=0x%p, attr=0x%p, curs=0x%p, fb=0x%p"
 811                "graphics extension=%u, extended_fontset=%d, blinking=%d\n",
 812                gw,gh,fw,fh,tw,th,cw,ch,px,py,font,text,attr,curs,fb,
 813                vga->vga_attribute_controller.vga_attribute_mode_control.enable_line_graphics_char_code,
 814                extended_fontset(vga), blinking(vga));
 815
 816     text_start=text;
 817
 818     // First we need to clip to what we can actually show
 819     rtw = tw < fw/cw ? tw : fw/cw;
 820     rth = th < fh/ch ? th : fh/ch;
 821
 822
 823
 824     // Now let's scan by char across the whole thing
 825     for (y=0;y<th;y++) {
 826         for (x=0;x<tw;x++, text++, attr++) {
 827             if (x < rtw && y < rth) {
 828                 // grab the character and attribute for the position
 829                 ct = *text;
 830                 ca = *attr;
 831                 a.val = ca;
 832
 833                 // find the character's font bitmap (one byte per row)
 834                 find_text_font_start(vga,(void**)&font,
 835                                      extended_fontset(vga) ? a.foreground_intensity_or_font_select : 0 );
 836
 837                 font += ct * ((VGA_MAX_FONT_HEIGHT * VGA_FONT_WIDTH)/8);
 838
 839                 // Now let's find out what colors we will be using
 840                 // foreground
 841
 842                 if (!extended_fontset(vga)) {
 843                     fg_entry = a.foreground_intensity_or_font_select << 3;
 844                 } else {
 845                     fg_entry = 0;
 846                 }
 847                 fg_entry |= a.fore;
 848
 849                 find_24bit_color(vga,fg_entry,&fgr,&fgg,&fgb);
 850
 851                 if (!blinking(vga)) {
 852                     bg_entry = a.blinking_or_bg_intensity << 3;
 853                 } else {
 854                     bg_entry = 0;
 855                 }
 856                 bg_entry |= a.back;
 857
 858                 find_24bit_color(vga,bg_entry,&bgr,&bgg,&bgb);
 859
 860                 // Draw the character
 861                 for (l=0; l<ch; l++, font++) {
 862                     uint8_t frow = *font;  // this is the row of of the font map
 863                     for (p=0;p<cw;p++) {
 864                         uint8_t fbit;
 865
 866                         // a char can be 9 bits wide, but the font map
 867                         // is only 8 bits wide, which means we need to know where to
 868                         // get the 9th bit
 869                         if (p >= 8) {
 870                             // We get it from the font map if
 871                             // its line line graphics mode and its a graphics char
 872                             // otherwise it's the background color
 873                             if (vga->vga_attribute_controller.vga_attribute_mode_control.enable_line_graphics_char_code
 874                                 && ct>=0xc0 && ct<=0xdf ) {
 875                                 fbit = frow & 0x1;
 876                             } else {
 877                                 fbit = 0;
 878                             }
 879                         } else {
 880                             fbit= (frow >> (7-p) ) & 0x1;
 881                         }
 882
 883                         // We are now at the pixel level, with fbit being the pixel we draw (color+attr or bg+attr)
 884                         // For now, we will draw it as black/white
 885
 886                         // find its position in the framebuffer;
 887                         fx = x*cw + p;
 888                         fy = y*ch + l;
 889                         pixel =  fb + ((fx + (fy*spec->width)) * spec->bytes_per_pixel);
 890                         red = pixel + spec->red_offset;
 891                         green = pixel + spec->green_offset;
 892                         blue = pixel + spec->blue_offset;
 893
 894                         // Are we on the cursor?
 895                         // if so, let's negate this pixel to invert the cell
 896                         if (curs==text) {
 897                             fbit^=0x1;
 898                         }
 899                         // update the framebuffer
 900                         if (fbit) {
 901                             *red=fgr;
 902                             *green=fgg;
 903                             *blue=fgb;
 904                         } else {
 905                             *red=bgr;
 906                             *green=bgg;
 907                             *blue=bgb;
 908                         }
 909                     }
 910                 }
 911             }
 912         }
 913         PrintDebug("\n");
 914     }
 915
 916 }
 917
 918
 919
 920
 921
 922 static void render_test(struct vga_internal *vga, void *fb)
 923 {
 924     struct v3_frame_buffer_spec *s;
 925
 926     s=&(vga->target_spec);
 927
 928     if (fb && s->height>=480 && s->width>=640 ) {
 929         uint8_t color = (uint8_t)(vga->updates_since_render);
 930
 931         uint32_t x, y;
 932
 933         for (y=0;y<480;y++) {
 934             for (x=0;x<640;x++) {
 935                 void *pixel = fb + ((x + (y*s->width)) * s->bytes_per_pixel);
 936                 uint8_t *red = pixel + s->red_offset;
 937                 uint8_t *green = pixel + s->green_offset;
 938                 uint8_t *blue = pixel + s->blue_offset;
 939
 940                 if (y<(480/4)) {
 941                     *red=color+x;
 942                     *green=0;
 943                     *blue=0;
 944                 } else if (y<(480/2)) {
 945                     *red=0;
 946                     *green=color+x;
 947                     *blue=0;
 948                 } else if (y<(3*(480/4))) {
 949                     *red=0;
 950                     *green=0;
 951                     *blue=color+x;
 952                 } else {
 953                     *red=*green=*blue=color+x;
 954                 }
 955             }
 956         }
 957     }
 958 }
 959
 960 static void render_black(struct vga_internal *vga, void *fb)
 961 {
 962     struct v3_frame_buffer_spec *s;
 963
 964     s=&(vga->target_spec);
 965
 966     memset(fb,0,s->height*s->width*s->bytes_per_pixel);
 967 }
 968
 969 static void render_maps(struct vga_internal *vga, void *fb)
 970 {
 971
 972     struct v3_frame_buffer_spec *s;
 973
 974
 975     s=&(vga->target_spec);
 976
 977     if (fb && s->height>=768 && s->width>=1024 && !(vga->updates_since_render % 100)) {
 978         // we draw the maps next, each being a 256x256 block appearing 32 pixels below the display block
 979         uint8_t m;
 980         uint32_t x,y;
 981         uint8_t *b;
 982
 983         for (m=0;m<4;m++) {
 984             b=(vga->map[m]);
 985             for (y=480+32;y<768;y++) {
 986                 for (x=m*256;x<(m+1)*256;x++,b++) {
 987                     void *pixel = fb + ((x + (y*s->width)) * s->bytes_per_pixel);
 988                     uint8_t *red = pixel + s->red_offset;
 989                     uint8_t *green = pixel + s->green_offset;
 990                     uint8_t *blue = pixel + s->blue_offset;
 991
 992                     *red=*green=*blue=*b;
 993                 }
 994             }
 995         }
 996     }
 997 }
 998
 999
1000 static int render(struct vga_internal *vga)
1001 {
1002     void *fb;
1003
1004
1005     vga->updates_since_render++;
1006
1007     if (vga->updates_since_render%100) {
1008         // skip render
1009         return 0;
1010     }
1011
1012     if (vga->host_cons && v3_graphics_console_inform_update(vga->host_cons)>0) {
1013
1014         fb = v3_graphics_console_get_frame_buffer_data_rw(vga->host_cons,&(vga->target_spec));
1015
1016         if (!(vga->vga_sequencer.vga_clocking_mode.screen_off)) {
1017             if (vga->vga_attribute_controller.vga_attribute_mode_control.graphics) {
1018                 render_graphics(vga,fb);
1019             } else {
1020                 render_text(vga,fb);
1021                 render_text_cursor(vga,fb);
1022             }
1023         } else {
1024             render_black(vga,fb);
1025         }
1026
1027         if (0) { render_test(vga,fb); }
1028
1029         // always render maps for now
1030         render_maps(vga,fb);
1031
1032         v3_graphics_console_release_frame_buffer_data_rw(vga->host_cons);
1033     }
1034
1035     return 0;
1036 }
1037
1038
1039 static void get_mem_region(struct vga_internal *vga, uint64_t *mem_start, uint64_t *mem_end)
1040 {
1041     switch (vga->vga_graphics_controller.vga_misc.memory_map) {
1042         case 0:
1043             *mem_start=0xa0000;
1044             *mem_end=0xc0000;
1045             break;
1046         case 1:
1047             *mem_start=0xa0000;
1048             *mem_end=0xb0000;
1049             break;
1050         case 2:
1051             *mem_start=0xb0000;
1052             *mem_end=0xb8000;
1053             break;
1054         case 3:
1055             *mem_start=0xb8000;
1056             *mem_end=0xc0000;
1057             break;
1058     }
1059 }
1060
1061 static uint64_t find_offset_write(struct vga_internal *vga, addr_t guest_addr)
1062 {
1063     uint64_t mem_start, mem_end;
1064     uint64_t size;
1065
1066     mem_start=mem_end=0;
1067
1068     get_mem_region(vga, &mem_start, &mem_end);
1069
1070     size=(mem_end-mem_start > 65536 ? 65536 : (mem_end-mem_start));
1071
1072     if (vga->vga_sequencer.vga_mem_mode.odd_even) {
1073         return (guest_addr-mem_start) % size;
1074     } else {
1075         // odd/even mode
1076         return ((guest_addr-mem_start) >> 1 ) % size;
1077     }
1078 }
1079
1080
1081
1082
1083 // Determines which maps should be enabled for this single byte write
1084 // and what the increment (actually 1/increment for the copy loop
1085 //
1086 //  memory_mode.odd_even == 0 => even address = maps 0 and 2 enabled; 1,3 otherwise
1087 //
1088 static uint8_t find_map_write(struct vga_internal *vga, addr_t guest_addr)
1089 {
1090     uint8_t mm = vga->vga_sequencer.vga_map_mask.val;
1091
1092     if (vga->vga_sequencer.vga_mem_mode.odd_even) {
1093         return mm;
1094     } else {
1095         if (guest_addr & 0x1) {
1096             return mm & 0xa;  // 0x1010
1097         } else {
1098             return mm & 0x5;  // 0x0101
1099         }
1100     }
1101 }
1102
1103 static uint8_t find_increment_write(struct vga_internal *vga, addr_t new_guest_addr)
1104 {
1105     if (vga->vga_sequencer.vga_mem_mode.odd_even) {
1106         return 1;
1107     } else {
1108         return !(new_guest_addr & 0x1);
1109     }
1110 }
1111
1112
1113
1114 static int vga_write(struct guest_info * core,
1115                      addr_t guest_addr,
1116                      void * src,
1117                      uint_t length,
1118                      void * priv_data)
1119 {
1120     struct vm_device *dev = (struct vm_device *)priv_data;
1121     struct vga_internal *vga = (struct vga_internal *) dev->private_data;
1122
1123     PrintDebug("vga: memory write: guest_addr=0x%p len=%u\n",(void*)guest_addr, length);
1124
1125     if (vga->passthrough) {
1126         PrintDebug("vga: passthrough write to 0x%p\n", V3_VAddr((void*)guest_addr));
1127         memcpy(V3_VAddr((void*)guest_addr),src,length);
1128     }
1129
1130 #if DEBUG_MEM_DATA
1131     int i;
1132     PrintDebug("vga: data written was 0x");
1133     for (i=0;i<length;i++) {
1134         uint8_t c= ((char*)src)[i];
1135         PrintDebug("%.2x", c);
1136     }
1137     PrintDebug(" \"");
1138     for (i=0;i<length;i++) {
1139         char c= ((char*)src)[i];
1140         PrintDebug("%c", (c>='a' && c<='z') || (c>='A' && c<='Z') || (c>='0' && c<='9') || (c==' ') ? c : '.');
1141     }
1142     PrintDebug("\"\n");
1143 #endif
1144
1145     /* Write mode determine by Graphics Mode Register (Index 05h).writemode */
1146
1147
1148     switch (vga->vga_graphics_controller.vga_graphics_mode.write_mode) {
1149         case 0: {
1150
1151             /*
1152                00b -- Write Mode 0: In this mode, the host data is first rotated
1153                as per the Rotate Count field, then the Enable Set/Reset mechanism
1154                selects data from this or the Set/Reset field. Then the selected
1155                Logical Operation is performed on the resulting data and the data
1156                in the latch register. Then the Bit Mask field is used to select
1157                which bits come from the resulting data and which come
1158                from the latch register. Finally, only the bit planes enabled by
1159                the Memory Plane Write Enable field are written to memory.
1160             */
1161
1162             int i;
1163
1164             uint8_t  mapnum;
1165             uint64_t offset;
1166
1167             uint8_t ror = vga->vga_graphics_controller.vga_data_rotate.rotate_count;
1168             uint8_t func = vga->vga_graphics_controller.vga_data_rotate.function;
1169
1170             offset = find_offset_write(vga, guest_addr);
1171
1172 #if DEBUG_DEEP_MEM
1173             PrintDebug("vga: mode 0 write, offset=0x%llx, ror=%u, func=%u\n", offset,ror,func);
1174 #endif
1175
1176             for (i=0;i<length;i++,offset+=find_increment_write(vga,guest_addr+i)) {
1177                 // now for each map
1178                 uint8_t sr = vga->vga_graphics_controller.vga_set_reset.val & 0xf;
1179                 uint8_t esr = vga->vga_graphics_controller.vga_enable_set_reset.val &0xf;
1180                 uint8_t bm = vga->vga_graphics_controller.vga_bit_mask;
1181                 uint8_t mm = find_map_write(vga,guest_addr+i);
1182
1183 #if DEBUG_DEEP_MEM
1184                 PrintDebug("vga: write i=%u, mm=0x%x, bm=0x%x sr=0x%x esr=0x%x offset=0x%x\n",i,(unsigned int)mm,(unsigned int)bm, (unsigned int)sr, (unsigned int)esr,(unsigned int)offset);
1185 #endif
1186
1187                 for (mapnum=0;mapnum<4;mapnum++, sr>>=1, esr>>=1, mm>>=1) {
1188                     vga_map map = vga->map[mapnum];
1189                     uint8_t data = ((uint8_t *)src)[i];
1190                     uint8_t latchval = vga->latch[mapnum];
1191
1192 #if DEBUG_DEEP_MEM
1193                     PrintDebug("vga: raw data=0x%x\n",data);
1194 #endif
1195                     // rotate data right
1196                     if (ror) {
1197                         data = (data>>ror) | data<<(8-ror);
1198                     }
1199
1200 #if DEBUG_DEEP_MEM
1201                     PrintDebug("vga: data after ror=0x%x\n",data);
1202 #endif
1203                     // use SR bit if ESR is on for this map
1204                     if (esr & 0x1) {
1205                         data = (sr&0x1) * -1;
1206
1207                     }
1208
1209 #if DEBUG_DEEP_MEM
1210                     PrintDebug("vga: data after esrr=0x%x\n",data);
1211 #endif
1212
1213                     // Apply function
1214                     switch (func) {
1215                         case 0: // NOP
1216                             break;
1217                         case 1: // AND
1218                             data &= latchval;
1219                             break;
1220                         case 2: // OR
1221                             data |= latchval;
1222                             break;
1223                         case 3: // XOR
1224                             data ^= latchval;
1225                             break;
1226                     }
1227
1228 #if DEBUG_DEEP_MEM
1229                     PrintDebug("vga: data after func=0x%x\n",data);
1230 #endif
1231
1232                     // mux between the data byte and the latch byte on
1233                     // a per-bit basis
1234                     data = (bm & data) | ((~bm) & latchval);
1235
1236
1237 #if DEBUG_DEEP_MEM
1238                     PrintDebug("vga: data after bm mux=0x%x\n",data);
1239 #endif
1240
1241                     // selective write
1242                     if (mm & 0x1) {
1243                         // write to this map
1244 #if DEBUG_DEEP_MEM
1245                         PrintDebug("vga: write map %u offset 0x%p map=0x%p pointer=0x%p\n",mapnum,(void*)offset,map,&(map[offset]));
1246 #endif
1247                         map[offset] = data;
1248                     } else {
1249                         // skip this map
1250                     }
1251                 }
1252             }
1253         }
1254             break;
1255
1256
1257
1258         case 1: {
1259             /*
1260                01b -- Write Mode 1: In this mode, data is transferred directly
1261                from the 32 bit latch register to display memory, affected only by
1262                the Memory Plane Write Enable field. The host data is not used in this mode.
1263             */
1264
1265             int i;
1266
1267             uint64_t offset = find_offset_write(vga,guest_addr);
1268
1269 #if DEBUG_DEEP_MEM
1270             PrintDebug("vga: mode 1 write, offset=0x%llx\n", offset);
1271 #endif
1272
1273             for (i=0;i<length;i++,offset+=find_increment_write(vga,guest_addr+i)) {
1274
1275                 uint8_t mapnum;
1276                 uint8_t mm = find_map_write(vga,guest_addr+i);
1277
1278                 for (mapnum=0;mapnum<4;mapnum++,  mm>>=1) {
1279                     vga_map map = vga->map[mapnum];
1280                     uint8_t latchval = vga->latch[mapnum];
1281
1282                     // selective write
1283                     if (mm & 0x1) {
1284                         // write to this map
1285                         map[offset] = latchval;
1286                     } else {
1287                         // skip this map
1288                     }
1289                 }
1290             }
1291         }
1292             break;
1293
1294         case 2: {
1295             /*
1296               10b -- Write Mode 2: In this mode, the bits 3-0 of the host data
1297               are replicated across all 8 bits of their respective planes.
1298               Then the selected Logical Operation is performed on the resulting
1299               data and the data in the latch register. Then the Bit Mask field is used to
1300               select which bits come from the resulting data and which come from
1301               the latch register. Finally, only the bit planes enabled by the
1302               Memory Plane Write Enable field are written to memory.
1303             */
1304             int i;
1305             uint8_t  mapnum;
1306             uint64_t offset;
1307
1308             uint8_t func = vga->vga_graphics_controller.vga_data_rotate.function;
1309
1310             offset = find_offset_write(vga, guest_addr);
1311
1312 #if DEBUG_DEEP_MEM
1313             PrintDebug("vga: mode 2 write, offset=0x%llx, func=%u\n", offset,func);
1314 #endif
1315
1316             for (i=0;i<length;i++,offset+=find_increment_write(vga,guest_addr+i)) {
1317                 // now for each map
1318                 uint8_t bm = vga->vga_graphics_controller.vga_bit_mask;
1319                 uint8_t mm = find_map_write(vga,guest_addr+i);
1320
1321                 for (mapnum=0;mapnum<4;mapnum++,  bm>>=1, mm>>=1) {
1322                     vga_map map = vga->map[mapnum];
1323                     uint8_t data = ((uint8_t *)src)[i];
1324                     uint8_t latchval = vga->latch[mapnum];
1325
1326                     // expand relevant bit to 8 bit
1327                     // it's basically esr=1, sr=bit from mode 0 write
1328                     data = ((data>>mapnum)&0x1) * -1;
1329
1330                     // Apply function
1331                     switch (func) {
1332                         case 0: // NOP
1333                             break;
1334                         case 1: // AND
1335                             data &= latchval;
1336                             break;
1337                         case 2: // OR
1338                             data |= latchval;
1339                             break;
1340                         case 3: // XOR
1341                             data ^= latchval;
1342                             break;
1343                     }
1344
1345                     // mux between latch and alu output
1346                     data = (bm & data) | ((~bm) & latchval);
1347
1348                     // selective write
1349                     if (mm & 0x1) {
1350                         // write to this map
1351                         map[offset] = data;
1352                     } else {
1353                         // skip this map
1354                     }
1355                 }
1356             }
1357         }
1358             break;
1359
1360         case 3: {
1361             /* 11b -- Write Mode 3: In this mode, the data in the Set/Reset field is used
1362                as if the Enable Set/Reset field were set to 1111b. Then the host data is
1363                first rotated as per the Rotate Count field, then logical ANDed with the
1364                value of the Bit Mask field. The resulting value is used on the data
1365                obtained from the Set/Reset field in the same way that the Bit Mask field
1366                would ordinarily be used. to select which bits come from the expansion
1367                of the Set/Reset field and which come from the latch register. Finally,
1368                only the bit planes enabled by the Memory Plane Write Enable field
1369                are written to memory.
1370             */
1371             int i;
1372
1373             uint8_t  mapnum;
1374             uint64_t offset;
1375
1376             uint8_t ror = vga->vga_graphics_controller.vga_data_rotate.rotate_count;
1377
1378             offset = find_offset_write(vga, guest_addr);
1379
1380             PrintDebug("vga: mode 3 write, offset=0x%llx, ror=%u\n", offset,ror);
1381
1382             for (i=0;i<length;i++,offset+=find_increment_write(vga,guest_addr+i)) {
1383                 // now for each map
1384                 uint8_t data = ((uint8_t *)src)[i];
1385
1386                 if (ror) {
1387                     data = (data>>ror) | data<<(8-ror);
1388                 }
1389
1390                 uint8_t bm = vga->vga_graphics_controller.vga_bit_mask & data;
1391                 uint8_t sr = vga->vga_graphics_controller.vga_set_reset.val & 0xf;
1392                 uint8_t mm = find_map_write(vga,guest_addr+i);
1393
1394                 for (mapnum=0;mapnum<4;mapnum++, sr>>=1, bm>>=1, mm>>=1) {
1395                     vga_map map = vga->map[mapnum];
1396                     uint8_t latchval = vga->latch[mapnum];
1397
1398                     // expand SR bit
1399                     data = (sr&0x1) * -1;
1400
1401                     // mux between latch and alu output
1402                     data = (bm & data) | ((~bm) & latchval);
1403
1404                     // selective write
1405                     if (mm & 0x1) {
1406                         // write to this map
1407                         map[offset] = data;
1408                     } else {
1409                         // skip this map
1410                     }
1411                 }
1412             }
1413
1414         }
1415             break;
1416
1417             // There is no default
1418     }
1419
1420     render(vga);
1421
1422     return length;
1423 }
1424
1425
1426
1427 static uint64_t find_offset_read(struct vga_internal *vga, addr_t guest_addr)
1428 {
1429     uint64_t mem_start, mem_end;
1430     uint64_t size;
1431
1432     mem_start=mem_end=0;
1433
1434     get_mem_region(vga, &mem_start, &mem_end);
1435
1436     size=(mem_end-mem_start > 65536 ? 65536 : (mem_end-mem_start));
1437
1438     if (!vga->vga_sequencer.vga_mem_mode.chain4) {
1439         return (guest_addr-mem_start) % size;
1440     } else {
1441         // chain4 mode
1442         return ((guest_addr - mem_start) >> 2) % size;
1443     }
1444 }
1445
1446 static uint8_t find_increment_read(struct vga_internal *vga, addr_t new_guest_addr)
1447 {
1448
1449     if (vga->vga_sequencer.vga_mem_mode.chain4) {
1450         return !(new_guest_addr & 0x3);
1451     } else {
1452         return 1;
1453     }
1454 }
1455
1456
1457 static int vga_read(struct guest_info * core,
1458                     addr_t guest_addr,
1459                     void * dst,
1460                     uint_t length,
1461                     void * priv_data)
1462 {
1463     struct vm_device *dev = (struct vm_device *)priv_data;
1464     struct vga_internal *vga = (struct vga_internal *) dev->private_data;
1465
1466
1467     PrintDebug("vga: memory read: guest_addr=0x%p len=%u\n",(void*)guest_addr, length);
1468
1469
1470
1471     /*
1472       Reading, 2 modes, set via Graphics Mode Register (index 05h).Read Mode:
1473     */
1474     switch (vga->vga_graphics_controller.vga_graphics_mode.read_mode) {
1475         case 0: {
1476             /*      0 - a byte from ONE of the 4 planes is returned;
1477                     which plane is determined by Read Map Select (Read Map Select Register (Index 04h)) */
1478             uint8_t  mapnum;
1479             uint64_t offset;
1480
1481
1482             if (vga->vga_sequencer.vga_mem_mode.chain4) {
1483                 uint32_t i;
1484                 offset = find_offset_read(vga,guest_addr);
1485                 // address bytes select the map
1486                 for (i=0;i<length;i++,offset+=find_increment_read(vga,guest_addr+i)) {
1487                     mapnum = (guest_addr+i) % 4;
1488 #if DEBUG_DEEP_MEM
1489                     PrintDebug("vga: mode 0 read, chain4, offset=0x%llx, mapnum=%u\n",offset,mapnum);
1490 #endif
1491                     ((uint8_t*)dst)[i] = *(vga->map[mapnum]+offset);
1492
1493                     // presumably all the latches are to be reloaded, not just the selected one?
1494                     for (mapnum=0;mapnum<4;mapnum++) {
1495                         vga->latch[mapnum] = *(vga->map[mapnum]+offset);
1496                     }
1497                 }
1498             } else {
1499                 mapnum = vga->vga_graphics_controller.vga_read_map_select.map_select;
1500                 offset = find_offset_read(vga,guest_addr);
1501
1502                 if (offset>=65536) {
1503                     PrintError("vga: read to offset=%llu map=%u (%u bytes)\n",offset,mapnum,length);
1504                 }
1505
1506 #if DEBUG_DEEP_MEM
1507                 PrintDebug("vga: mode 0 read, not-chain4, offset=0x%llx, mapnum=%u\n",offset,mapnum);
1508 #endif
1509
1510                 memcpy(dst,(vga->map[mapnum])+offset,length);
1511
1512                 // load the latches with the last item read
1513                 for (mapnum=0;mapnum<4;mapnum++) {
1514                     vga->latch[mapnum] = vga->map[mapnum][offset+length-1];
1515                 }
1516             }
1517
1518         }
1519             break;
1520         case 1: {
1521             /*      1 - Compare video memory and reference color
1522                     (in Color Compare, except those not set in Color Don't Care),
1523                     each bit in returned result is one comparison between the reference color
1524
1525                     Ref color is *4* bits, and thus a one byte read returns a comparison
1526                     with 8 pixels
1527
1528             */
1529             int i;
1530
1531             uint8_t cc=vga->vga_graphics_controller.vga_color_compare.val & 0xf ;
1532             uint8_t dc=vga->vga_graphics_controller.vga_color_dont_care.val & 0xf;
1533
1534             uint8_t  mapnum;
1535             uint64_t offset;
1536             uint8_t  byte;
1537             uint8_t  bits;
1538
1539             offset = find_offset_read(vga,guest_addr);
1540
1541 #if DEBUG_DEEP_MEM
1542             PrintDebug("vga: mode 1 read, offset=0x%llx, cc=0x%x, dc-0x%x\n",offset,cc,dc);
1543 #endif
1544
1545
1546             for (i=0;i<length;i++,offset++) {
1547                 vga_map map;
1548                 byte=0;
1549                 for (mapnum=0;mapnum<4;mapnum++) {
1550                     map = vga->map[mapnum];
1551                     if ( (dc>>mapnum)&0x1 ) { // don't care
1552                         bits=0;
1553                     } else {
1554                         // lower 4 bits
1555                         bits = (map[offset]&0xf) == cc;
1556                         bits <<= 1;
1557                         // upper 4 bits
1558                         bits |= (((map[offset]>>4))&0xf) == cc;
1559                     }
1560                     // not clear whether it is 0..k or k..0
1561                     byte<<=2;
1562                     byte|=bits;
1563                 }
1564             }
1565
1566             // load the latches with the last item read
1567             for (mapnum=0;mapnum<4;mapnum++) {
1568                 vga->latch[mapnum] = vga->map[mapnum][offset+length-1];
1569             }
1570
1571         }
1572             break;
1573             // there is no default
1574     }
1575
1576     if (vga->passthrough) {
1577         PrintDebug("vga: passthrough read from 0x%p\n",V3_VAddr((void*)guest_addr));
1578         memcpy(dst,V3_VAddr((void*)guest_addr),length);
1579     }
1580
1581
1582 #if DEBUG_MEM_DATA
1583     int i;
1584     PrintDebug("vga: data read is 0x");
1585     for (i=0;i<length;i++) {
1586         uint8_t c= ((char*)dst)[i];
1587         PrintDebug("%.2x", c);
1588     }
1589     PrintDebug(" \"");
1590     for (i=0;i<length;i++) {
1591         char c= ((char*)dst)[i];
1592         PrintDebug("%c", (c>='a' && c<='z') || (c>='A' && c<='Z') || (c>='0' && c<='9') || (c==' ') ? c : '.');
1593     }
1594     PrintDebug("\"\n");
1595 #endif
1596
1597     return length;
1598
1599 }
1600
1601
1602
1603
1604
1605 #define ERR_WRONG_SIZE(op,reg,len,min,max)      \
1606     if (((len)<(min)) || ((len)>(max))) {       \
1607         PrintError("vga: %s of %s wrong size (%d bytes, but only %d to %d allowed)\n",(op),(reg),(len),(min),(max)); \
1608         return -1; \
1609 }
1610
1611 static inline void passthrough_io_in(uint16_t port, void * dest, uint_t length) {
1612     switch (length) {
1613         case 1:
1614             *(uint8_t *)dest = v3_inb(port);
1615             break;
1616         case 2:
1617             *(uint16_t *)dest = v3_inw(port);
1618             break;
1619         case 4:
1620             *(uint32_t *)dest = v3_indw(port);
1621             break;
1622         default:
1623             PrintError("vga: unsupported passthrough io in size %u\n",length);
1624             break;
1625     }
1626 }
1627
1628
1629 static inline void passthrough_io_out(uint16_t port, const void * src, uint_t length) {
1630     switch (length) {
1631         case 1:
1632             v3_outb(port, *(uint8_t *)src);
1633             break;
1634         case 2:
1635             v3_outw(port, *(uint16_t *)src);
1636             break;
1637         case 4:
1638             v3_outdw(port, *(uint32_t *)src);
1639             break;
1640         default:
1641             PrintError("vga: unsupported passthrough io out size %u\n",length);
1642             break;
1643     }
1644 }
1645
1646 #define PASSTHROUGH_IO_IN(vga,port,dest,len)                            \
1647     do { if ((vga)->passthrough) { passthrough_io_in(port,dest,len); } } while (0)
1648
1649 #define PASSTHROUGH_IO_OUT(vga,port,src,len)                            \
1650     do { if ((vga)->passthrough && (!(vga)->skip_next_passthrough_out)) { passthrough_io_out(port,src,len); } (vga)->skip_next_passthrough_out=false; } while (0)
1651
1652 #define PASSTHROUGH_IO_SKIP_NEXT_OUT(vga)                                       \
1653     do { if ((vga)->passthrough) { (vga)->skip_next_passthrough_out=true; } } while (0)
1654
1655 #define PASSTHROUGH_READ_CHECK(vga,inter,pass) \
1656     do { if ((vga)->passthrough) { if ((inter)!=(pass)) { PrintError("vga: passthrough error: passthrough value read is 0x%x, but internal value read is 0x%x\n",(pass),(inter)); } } } while (0)
1657
1658 static int misc_out_read(struct guest_info *core,
1659                          uint16_t port,
1660                          void *dest,
1661                          uint_t len,
1662                          void *priv_data)
1663 {
1664     struct vga_internal *vga = (struct vga_internal *) priv_data;
1665
1666     PrintDebug("vga: misc out read data=0x%x\n", vga->vga_misc.vga_misc_out.val);
1667
1668     ERR_WRONG_SIZE("read","misc out",len,1,1);
1669
1670     *((uint8_t*)dest) = vga->vga_misc.vga_misc_out.val;
1671
1672     PASSTHROUGH_IO_IN(vga,port,dest,len);
1673
1674     PASSTHROUGH_READ_CHECK(vga,vga->vga_misc.vga_misc_out.val,*((uint8_t*)dest));
1675
1676     return len;
1677 }
1678
1679 static int misc_out_write(struct guest_info *core,
1680                           uint16_t port,
1681                           void *src,
1682                           uint_t len,
1683                           void *priv_data)
1684 {
1685     struct vga_internal *vga = (struct vga_internal *) priv_data;
1686
1687     PrintDebug("vga: misc out write data=0x%x\n", *((uint8_t*)src));
1688
1689     ERR_WRONG_SIZE("write","misc out",len,1,1);
1690
1691     PASSTHROUGH_IO_OUT(vga,port,src,len);
1692
1693     vga->vga_misc.vga_misc_out.val =  *((uint8_t*)src) ;
1694
1695     render(vga);
1696
1697     return len;
1698 }
1699
1700
1701
1702 static int input_stat0_read(struct guest_info *core,
1703                             uint16_t port,
1704                             void *dest,
1705                             uint_t len,
1706                             void *priv_data)
1707 {
1708     struct vga_internal *vga = (struct vga_internal *) priv_data;
1709
1710     PrintDebug("vga: input stat0  read data=0x%x\n", vga->vga_misc.vga_input_stat0.val);
1711
1712     ERR_WRONG_SIZE("read","input stat0",len,1,1);
1713
1714     *((uint8_t*)dest) = vga->vga_misc.vga_input_stat0.val;
1715
1716     PASSTHROUGH_IO_IN(vga,port,dest,len);
1717
1718     PASSTHROUGH_READ_CHECK(vga,vga->vga_misc.vga_input_stat0.val,*(uint8_t*)dest);
1719
1720     return len;
1721 }
1722
1723
1724 static int input_stat1_read(struct guest_info *core,
1725                             uint16_t port,
1726                             void *dest,
1727                             uint_t len,
1728                             void *priv_data)
1729 {
1730     struct vga_internal *vga = (struct vga_internal *) priv_data;
1731
1732     PrintDebug("vga: input stat0 (%s) read data=0x%x\n",
1733                port==0x3ba ? "mono" : "color",
1734                vga->vga_misc.vga_input_stat1.val);
1735
1736     ERR_WRONG_SIZE("read","input stat1",len,1,1);
1737
1738
1739     *((uint8_t*)dest) = vga->vga_misc.vga_input_stat1.val;
1740
1741     // Stunningly, reading stat1 is also a way to reset
1742     // the state of attribute controller address/data flipflop
1743     // That is some mighty fine crack the designers were smoking.
1744
1745     vga->vga_attribute_controller.state=ATTR_ADDR;
1746
1747     PASSTHROUGH_IO_IN(vga,port,dest,len);
1748
1749     PASSTHROUGH_READ_CHECK(vga,vga->vga_misc.vga_input_stat1.val,*(uint8_t*)dest);
1750
1751     return len;
1752 }
1753
1754
1755 static int feature_control_read(struct guest_info *core,
1756                                 uint16_t port,
1757                                 void *dest,
1758                                 uint_t len,
1759                                 void *priv_data)
1760 {
1761     struct vga_internal *vga = (struct vga_internal *) priv_data;
1762
1763     PrintDebug("vga: feature control  read data=0x%x\n",
1764                vga->vga_misc.vga_feature_control.val);
1765
1766     ERR_WRONG_SIZE("read","feature control",len,1,1);
1767
1768
1769     *((uint8_t*)dest) = vga->vga_misc.vga_feature_control.val;
1770
1771     PASSTHROUGH_IO_IN(vga,port,dest,len);
1772
1773     PASSTHROUGH_READ_CHECK(vga,vga->vga_misc.vga_feature_control.val,*(uint8_t*)dest);
1774
1775     return len;
1776 }
1777
1778 static int feature_control_write(struct guest_info *core,
1779                                  uint16_t port,
1780                                  void *src,
1781                                  uint_t len,
1782                                  void *priv_data)
1783 {
1784     struct vga_internal *vga = (struct vga_internal *) priv_data;
1785
1786     PrintDebug("vga: feature control (%s) write data=0x%x\n",
1787                port==0x3ba ? "mono" : "color",
1788                *((uint8_t*)src));
1789
1790     ERR_WRONG_SIZE("write","feature control",len,1,1);
1791
1792     PASSTHROUGH_IO_OUT(vga,port,src,len);
1793
1794     vga->vga_misc.vga_feature_control.val =  *((uint8_t*)src) ;
1795
1796     render(vga);
1797
1798     return len;
1799 }
1800
1801
1802 static int video_subsys_enable_read(struct guest_info *core,
1803                                     uint16_t port,
1804                                     void *dest,
1805                                     uint_t len,
1806                                     void *priv_data)
1807 {
1808     struct vga_internal *vga = (struct vga_internal *) priv_data;
1809
1810     PrintDebug("vga: video subsys enable read data=0x%x\n",
1811                vga->vga_misc.vga_video_subsys_enable.val);
1812
1813     ERR_WRONG_SIZE("read","video subsys enable",len,1,1);
1814
1815     *((uint8_t*)dest) = vga->vga_misc.vga_video_subsys_enable.val;
1816
1817     PASSTHROUGH_IO_IN(vga,port,dest,len);
1818
1819     PASSTHROUGH_READ_CHECK(vga,vga->vga_misc.vga_video_subsys_enable.val,*(uint8_t*)dest);
1820
1821     return len;
1822 }
1823
1824 static int video_subsys_enable_write(struct guest_info *core,
1825                                      uint16_t port,
1826                                      void *src,
1827                                      uint_t len,
1828                                      void *priv_data)
1829 {
1830     struct vga_internal *vga = (struct vga_internal *) priv_data;
1831
1832     PrintDebug("vga: video subsys enable write data=0x%x\n", *((uint8_t*)src));
1833
1834     ERR_WRONG_SIZE("write","video subsys enable",len,1,1);
1835
1836     PASSTHROUGH_IO_OUT(vga,port,src,len);
1837
1838     vga->vga_misc.vga_video_subsys_enable.val =  *((uint8_t*)src) ;
1839
1840     render(vga);
1841
1842     return len;
1843 }
1844
1845 static int sequencer_address_read(struct guest_info *core,
1846                                   uint16_t port,
1847                                   void *dest,
1848                                   uint_t len,
1849                                   void *priv_data)
1850 {
1851     struct vga_internal *vga = (struct vga_internal *) priv_data;
1852
1853     PrintDebug("vga: sequencer address read data=0x%x\n",
1854                vga->vga_sequencer.vga_sequencer_addr.val);
1855
1856     ERR_WRONG_SIZE("read","vga sequencer addr",len,1,1);
1857
1858     *((uint8_t*)dest) = vga->vga_sequencer.vga_sequencer_addr.val;
1859
1860     PASSTHROUGH_IO_IN(vga,port,dest,len);
1861
1862     PASSTHROUGH_READ_CHECK(vga,vga->vga_sequencer.vga_sequencer_addr.val,*(uint8_t*)dest);
1863
1864     return len;
1865 }
1866
1867 static int sequencer_data_write(struct guest_info *core,
1868                                uint16_t port,
1869                                void *src,
1870                                uint_t len,
1871                                void *priv_data)
1872 {
1873     struct vga_internal *vga = (struct vga_internal *) priv_data;
1874     uint8_t index;
1875     uint8_t data;
1876
1877     data=*((uint8_t*)src);
1878     index=vga->vga_sequencer.vga_sequencer_addr.val;  // should mask probably
1879
1880     PrintDebug("vga: sequencer write data (index=%d) with 0x%x\n",
1881                index, data);
1882
1883     ERR_WRONG_SIZE("write","vga sequencer data",len,1,1);
1884
1885     PASSTHROUGH_IO_OUT(vga,port,src,len);
1886
1887
1888     if (index>=VGA_SEQUENCER_NUM) {
1889         PrintError("vga: sequencer data write is for invalid index %d, ignoring\n",index);
1890     } else {
1891         vga->vga_sequencer.vga_sequencer_regs[index] = data;
1892     }
1893
1894     render(vga);
1895
1896     return len;
1897 }
1898
1899 static int sequencer_address_write(struct guest_info *core,
1900                                   uint16_t port,
1901                                   void *src,
1902                                   uint_t len,
1903                                   void *priv_data)
1904 {
1905     struct vga_internal *vga = (struct vga_internal *) priv_data;
1906     uint8_t new_addr;
1907
1908     new_addr=*((uint8_t*)src);
1909
1910     PrintDebug("vga: sequencer address write data=0x%x len=%u\n", len==1 ? *((uint8_t*)src) : len==2 ? *((uint16_t*)src) : *((uint32_t*)src), len);
1911
1912     ERR_WRONG_SIZE("write","vga sequencer addr",len,1,2);
1913
1914     PASSTHROUGH_IO_OUT(vga,port,src,len);
1915
1916     vga->vga_sequencer.vga_sequencer_addr.val =  *((uint8_t*)src) ;
1917
1918     if (len==2) {
1919         PASSTHROUGH_IO_SKIP_NEXT_OUT(vga);
1920         // second byte is the data
1921         if (sequencer_data_write(core,port,src+1,1,vga)!=1) {
1922             PrintError("vga: write of data failed\n");
1923             return -1;
1924         }
1925     }
1926
1927     return len;
1928 }
1929
1930 static int sequencer_data_read(struct guest_info *core,
1931                               uint16_t port,
1932                               void *dest,
1933                               uint_t len,
1934                               void *priv_data)
1935 {
1936     struct vga_internal *vga = (struct vga_internal *) priv_data;
1937     uint8_t index;
1938     uint8_t data;
1939
1940     index=vga->vga_sequencer.vga_sequencer_addr.val;  // should mask probably
1941
1942     if (index>=VGA_SEQUENCER_NUM) {
1943         data=0;
1944         PrintError("vga: sequencer data read at invalid index %d, returning zero\n",index);
1945     } else {
1946         data=vga->vga_sequencer.vga_sequencer_regs[index];
1947     }
1948
1949     PrintDebug("vga: sequencer data read data (index=%d) = 0x%x\n",
1950                    index, data);
1951
1952     ERR_WRONG_SIZE("read","vga sequencer data",len,1,1);
1953
1954     *((uint8_t*)dest) = data;
1955
1956     PASSTHROUGH_IO_IN(vga,port,dest,len);
1957
1958     PASSTHROUGH_READ_CHECK(vga,data,*(uint8_t*)dest);
1959
1960     return len;
1961 }
1962
1963
1964
1965
1966
1967 static int crt_controller_address_read(struct guest_info *core,
1968                                         uint16_t port,
1969                                         void *dest,
1970                                         uint_t len,
1971                                         void *priv_data)
1972 {
1973     struct vga_internal *vga = (struct vga_internal *) priv_data;
1974
1975     PrintDebug("vga: crt controller (%s) address read data=0x%x\n",
1976                port==0x3b4 ? "mono" : "color",
1977                vga->vga_crt_controller.vga_crt_addr.val);
1978
1979     ERR_WRONG_SIZE("read","vga crt controller addr",len,1,1);
1980
1981     *((uint8_t*)dest) = vga->vga_crt_controller.vga_crt_addr.val;
1982
1983     PASSTHROUGH_IO_IN(vga,port,dest,len);
1984
1985     PASSTHROUGH_READ_CHECK(vga,vga->vga_crt_controller.vga_crt_addr.val,*(uint8_t*)dest);
1986
1987     return len;
1988 }
1989
1990 static int crt_controller_data_write(struct guest_info *core,
1991                                      uint16_t port,
1992                                      void *src,
1993                                      uint_t len,
1994                                      void *priv_data)
1995 {
1996     struct vga_internal *vga = (struct vga_internal *) priv_data;
1997     uint8_t index;
1998     uint8_t data;
1999
2000     data=*((uint8_t*)src);
2001
2002     index=vga->vga_crt_controller.vga_crt_addr.val;  // should mask probably
2003
2004     PrintDebug("vga: crt controller (%s) write data (index=%d) with 0x%x\n",
2005                port==0x3b5 ? "mono" : "color",
2006                index, data);
2007
2008     ERR_WRONG_SIZE("write","vga crt controller data",len,1,1);
2009
2010     PASSTHROUGH_IO_OUT(vga,port,src,len);
2011
2012     if (index>=VGA_CRT_CONTROLLER_NUM) {
2013         PrintError("vga; crt controller write is for illegal index %d, ignoring\n",index);
2014     } else {
2015         vga->vga_crt_controller.vga_crt_controller_regs[index] = data;
2016     }
2017
2018     render(vga);
2019
2020     return len;
2021 }
2022
2023 static int crt_controller_address_write(struct guest_info *core,
2024                                         uint16_t port,
2025                                         void *src,
2026                                         uint_t len,
2027                                         void *priv_data)
2028 {
2029     struct vga_internal *vga = (struct vga_internal *) priv_data;
2030     uint8_t new_addr;
2031
2032     new_addr=*((uint8_t*)src);
2033
2034     PrintDebug("vga: crt controller (%s) address write data=0x%x len=%u\n",
2035                port==0x3b4 ? "mono" : "color",
2036                len==1 ? *((uint8_t*)src) : len==2 ? *((uint16_t*)src) : *((uint32_t*)src), len);
2037
2038     ERR_WRONG_SIZE("write","vga crt controller addr",len,1,2);
2039
2040     PASSTHROUGH_IO_OUT(vga,port,src,len);
2041
2042     vga->vga_crt_controller.vga_crt_addr.val =  *((uint8_t*)src) ;
2043
2044     if (len==2) {
2045         PASSTHROUGH_IO_SKIP_NEXT_OUT(vga);
2046         // second byte is the data
2047         if (crt_controller_data_write(core,port,src+1,1,vga)!=1) {
2048             PrintError("vga: write of data failed\n");
2049             return -1;
2050         }
2051     }
2052
2053     return len;
2054 }
2055
2056 static int crt_controller_data_read(struct guest_info *core,
2057                                     uint16_t port,
2058                                     void *dest,
2059                                     uint_t len,
2060                                     void *priv_data)
2061 {
2062     struct vga_internal *vga = (struct vga_internal *) priv_data;
2063     uint8_t index;
2064     uint8_t data;
2065
2066     index=vga->vga_crt_controller.vga_crt_addr.val;  // should mask probably
2067
2068     if (index>=VGA_CRT_CONTROLLER_NUM) {
2069         data=0;
2070         PrintError("vga: crt controller data read for illegal index %d, returning zero\n",index);
2071     } else {
2072         data=vga->vga_crt_controller.vga_crt_controller_regs[index];
2073     }
2074
2075     PrintDebug("vga: crt controller data (index=%d) = 0x%x\n",index,data);
2076
2077     ERR_WRONG_SIZE("read","vga crt controller data",len,1,1);
2078
2079     *((uint8_t*)dest) = data;
2080
2081     PASSTHROUGH_IO_IN(vga,port,dest,len);
2082
2083     PASSTHROUGH_READ_CHECK(vga,data,*(uint8_t *)dest);
2084
2085     return len;
2086 }
2087
2088
2089
2090 static int graphics_controller_address_read(struct guest_info *core,
2091                                             uint16_t port,
2092                                             void *dest,
2093                                             uint_t len,
2094                                             void *priv_data)
2095 {
2096     struct vga_internal *vga = (struct vga_internal *) priv_data;
2097
2098     PrintDebug("vga: graphics controller address read data=0x%x\n",
2099                vga->vga_graphics_controller.vga_graphics_ctrl_addr.val);
2100
2101     ERR_WRONG_SIZE("read","vga graphics controller addr",len,1,1);
2102
2103     *((uint8_t*)dest) = vga->vga_graphics_controller.vga_graphics_ctrl_addr.val;
2104
2105     PASSTHROUGH_IO_IN(vga,port,dest,len);
2106
2107     PASSTHROUGH_READ_CHECK(vga,vga->vga_graphics_controller.vga_graphics_ctrl_addr.val,*(uint8_t*)dest);
2108
2109     return len;
2110 }
2111
2112 static int graphics_controller_data_write(struct guest_info *core,
2113                                           uint16_t port,
2114                                           void *src,
2115                                           uint_t len,
2116                                           void *priv_data)
2117 {
2118     struct vga_internal *vga = (struct vga_internal *) priv_data;
2119     uint8_t index;
2120     uint8_t data;
2121
2122     data=*((uint8_t*)src);
2123     index=vga->vga_graphics_controller.vga_graphics_ctrl_addr.val;  // should mask probably
2124
2125
2126     PrintDebug("vga: graphics_controller write data (index=%d) with 0x%x\n",
2127                index, data);
2128
2129     ERR_WRONG_SIZE("write","vga graphics controller data",len,1,1);
2130
2131     PASSTHROUGH_IO_OUT(vga,port,src,len);
2132
2133     if (index>=VGA_GRAPHICS_CONTROLLER_NUM) {
2134         PrintError("vga: graphics controller write for illegal index %d ignored\n",index);
2135     } else {
2136         vga->vga_graphics_controller.vga_graphics_controller_regs[index] = data;
2137     }
2138
2139     render(vga);
2140
2141     return len;
2142 }
2143
2144 static int graphics_controller_address_write(struct guest_info *core,
2145                                              uint16_t port,
2146                                              void *src,
2147                                              uint_t len,
2148                                              void *priv_data)
2149 {
2150     struct vga_internal *vga = (struct vga_internal *) priv_data;
2151     uint8_t new_addr;
2152
2153     new_addr=*((uint8_t*)src);
2154
2155     PrintDebug("vga: graphics controller address write data=0x%x len=%u\n",
2156                len==1 ? *((uint8_t*)src) : len==2 ? *((uint16_t*)src) : *((uint32_t*)src), len);
2157
2158     ERR_WRONG_SIZE("write","vga graphics controller addr",len,1,2);
2159
2160     PASSTHROUGH_IO_OUT(vga,port,src,len);
2161
2162     vga->vga_graphics_controller.vga_graphics_ctrl_addr.val =  *((uint8_t*)src) ;
2163
2164     if (len==2) {
2165         PASSTHROUGH_IO_SKIP_NEXT_OUT(vga);
2166         // second byte is the data
2167         if (graphics_controller_data_write(core,port,src+1,1,vga)!=1) {
2168             PrintError("vga: write of data failed\n");
2169             return -1;
2170         }
2171     }
2172
2173     return len;
2174 }
2175
2176 static int graphics_controller_data_read(struct guest_info *core,
2177                                          uint16_t port,
2178                                          void *dest,
2179                                          uint_t len,
2180                                          void *priv_data)
2181 {
2182     struct vga_internal *vga = (struct vga_internal *) priv_data;
2183     uint8_t index;
2184     uint8_t data;
2185
2186     index=vga->vga_graphics_controller.vga_graphics_ctrl_addr.val;  // should mask probably
2187
2188
2189     if (index>=VGA_GRAPHICS_CONTROLLER_NUM) {
2190         data=0;
2191         PrintError("vga: graphics controller data read from illegal index %d, returning zero\n",index);
2192     } else {
2193         data=vga->vga_graphics_controller.vga_graphics_controller_regs[index];
2194     }
2195
2196     PrintDebug("vga: graphics controller data read data (index=%d) = 0x%x\n",
2197                index, data);
2198
2199     ERR_WRONG_SIZE("read","vga graphics controller data",len,1,1);
2200
2201     *((uint8_t*)dest) = data;
2202
2203     PASSTHROUGH_IO_IN(vga,port,dest,len);
2204
2205     PASSTHROUGH_READ_CHECK(vga,data,*(uint8_t*)dest);
2206
2207     return len;
2208 }
2209
2210
2211
2212
2213 /* Note that these guys have a bizarre protocol*/
2214
2215 static int attribute_controller_address_read(struct guest_info *core,
2216                                              uint16_t port,
2217                                              void *dest,
2218                                              uint_t len,
2219                                              void *priv_data)
2220 {
2221     struct vga_internal *vga = (struct vga_internal *) priv_data;
2222
2223     PrintDebug("vga: attribute controller address read data=0x%x\n",
2224                vga->vga_attribute_controller.vga_attribute_controller_addr.val);
2225
2226     ERR_WRONG_SIZE("read","vga attribute  controller addr",len,1,1);
2227
2228     *((uint8_t*)dest) = vga->vga_attribute_controller.vga_attribute_controller_addr.val;
2229
2230     PASSTHROUGH_IO_IN(vga,port,dest,len);
2231
2232     PASSTHROUGH_READ_CHECK(vga,vga->vga_attribute_controller.vga_attribute_controller_addr.val,*(uint8_t*)dest);
2233
2234     // Reading the attribute controller does not change the state
2235
2236     return len;
2237 }
2238
2239 static int attribute_controller_address_and_data_write(struct guest_info *core,
2240                                                        uint16_t port,
2241                                                        void *src,
2242                                                        uint_t len,
2243                                                        void *priv_data)
2244 {
2245     struct vga_internal *vga = (struct vga_internal *) priv_data;
2246
2247
2248     if (vga->vga_attribute_controller.state==ATTR_ADDR) {
2249         uint8_t new_addr = *((uint8_t*)src);
2250         // We are to treat this as an address write, and flip state
2251         // to expect data ON THIS SAME PORT
2252         PrintDebug("vga: attribute controller address write data=0x%x\n", new_addr);
2253
2254         ERR_WRONG_SIZE("write","vga attribute controller addr",len,1,1);
2255
2256         PASSTHROUGH_IO_OUT(vga,port,src,len);
2257
2258         vga->vga_attribute_controller.vga_attribute_controller_addr.val =  new_addr;
2259
2260         vga->vga_attribute_controller.state=ATTR_DATA;
2261         return len;
2262
2263     } else if (vga->vga_attribute_controller.state==ATTR_DATA) {
2264
2265         uint8_t data = *((uint8_t*)src);
2266         uint8_t index=vga->vga_attribute_controller.vga_attribute_controller_addr.val;  // should mask probably
2267
2268         PrintDebug("vga: attribute controller data write index %d with data=0x%x\n", index,data);
2269
2270         ERR_WRONG_SIZE("write","vga attribute controller data",len,1,1);
2271
2272         PASSTHROUGH_IO_OUT(vga,port,src,len);
2273
2274         if (index>=VGA_ATTRIBUTE_CONTROLLER_NUM) {
2275             PrintError("vga: attribute controller write to illegal index %d ignored\n",index);
2276         } else {
2277             vga->vga_attribute_controller.vga_attribute_controller_regs[index] = data;
2278         }
2279
2280         vga->vga_attribute_controller.state=ATTR_ADDR;
2281
2282         return len;
2283     }
2284
2285     return -1;
2286
2287 }
2288
2289 static int attribute_controller_data_read(struct guest_info *core,
2290                                           uint16_t port,
2291                                           void *dest,
2292                                           uint_t len,
2293                                           void *priv_data)
2294 {
2295     struct vga_internal *vga = (struct vga_internal *) priv_data;
2296     uint8_t index;
2297     uint8_t data;
2298
2299     index=vga->vga_attribute_controller.vga_attribute_controller_addr.val;  // should mask probably
2300
2301     if (index>=VGA_ATTRIBUTE_CONTROLLER_NUM) {
2302         data=0;
2303         PrintError("vga: attribute controller read of illegal index %d, returning zero\n",index);
2304     } else {
2305         data=vga->vga_attribute_controller.vga_attribute_controller_regs[index];
2306     }
2307
2308     PrintDebug("vga: attribute controller data read data (index=%d) = 0x%x\n",
2309                index, data);
2310
2311     ERR_WRONG_SIZE("read","vga attribute controller data",len,1,1);
2312
2313     *((uint8_t*)dest) = data;
2314
2315     PASSTHROUGH_IO_IN(vga,port,dest,len);
2316
2317     PASSTHROUGH_READ_CHECK(vga,data,*(uint8_t*)dest);
2318
2319     return len;
2320 }
2321
2322
2323 /*
2324    Note that these guys also have a strange protocol
2325    since they need to squeeze 18 bits of data through
2326    an 8 bit port
2327 */
2328 static int dac_write_address_read(struct guest_info *core,
2329                                   uint16_t port,
2330                                   void *dest,
2331                                   uint_t len,
2332                                   void *priv_data)
2333 {
2334     struct vga_internal *vga = (struct vga_internal *) priv_data;
2335
2336     PrintDebug("vga: dac write address read data=0x%x\n",
2337                vga->vga_dac.vga_dac_write_addr);
2338
2339     ERR_WRONG_SIZE("read","vga dac write addr",len,1,1);
2340
2341
2342     *((uint8_t*)dest) = vga->vga_dac.vga_dac_write_addr;
2343
2344     PASSTHROUGH_IO_IN(vga,port,dest,len);
2345
2346     PASSTHROUGH_READ_CHECK(vga,vga->vga_dac.vga_dac_write_addr,*(uint8_t*)dest);
2347
2348     // This read does not reset the state machine
2349
2350     return len;
2351 }
2352
2353 static int dac_write_address_write(struct guest_info *core,
2354                                    uint16_t port,
2355                                    void *src,
2356                                    uint_t len,
2357                                    void *priv_data)
2358 {
2359     struct vga_internal *vga = (struct vga_internal *) priv_data;
2360     uint8_t new_addr;
2361
2362     new_addr=*((uint8_t*)src);
2363
2364     PrintDebug("vga: dac write address write data=0x%x\n", new_addr);
2365
2366     ERR_WRONG_SIZE("write","vga dac write addr",len,1,1);
2367
2368     PASSTHROUGH_IO_OUT(vga,port,src,len);
2369
2370     // cannot be out of bounds since there are 256 regs
2371
2372     vga->vga_dac.vga_dac_write_addr =  *((uint8_t*)src) ;
2373
2374     // Now we also need to reset the state machine
2375
2376     vga->vga_dac.state=DAC_WRITE;
2377     vga->vga_dac.channel=RED;
2378
2379     return len;
2380 }
2381
2382
2383 static int dac_read_address_read(struct guest_info *core,
2384                                  uint16_t port,
2385                                  void *dest,
2386                                  uint_t len,
2387                                  void *priv_data)
2388 {
2389     struct vga_internal *vga = (struct vga_internal *) priv_data;
2390
2391     PrintDebug("vga: dac read address read data=0x%x\n",
2392                vga->vga_dac.vga_dac_read_addr);
2393
2394     ERR_WRONG_SIZE("read","vga dac read addr",len,1,1);
2395
2396     *((uint8_t*)dest) = vga->vga_dac.vga_dac_read_addr;
2397
2398     PASSTHROUGH_IO_IN(vga,port,dest,len);
2399
2400     PASSTHROUGH_READ_CHECK(vga,vga->vga_dac.vga_dac_read_addr,*(uint8_t*)dest);
2401
2402     // This read does not reset the state machine
2403
2404     return len;
2405 }
2406
2407 static int dac_read_address_write(struct guest_info *core,
2408                                   uint16_t port,
2409                                   void *src,
2410                                   uint_t len,
2411                                   void *priv_data)
2412 {
2413     struct vga_internal *vga = (struct vga_internal *) priv_data;
2414     uint8_t new_addr;
2415
2416     new_addr=*((uint8_t*)src);
2417
2418     PrintDebug("vga: dac read address write data=0x%x\n", new_addr);
2419
2420     ERR_WRONG_SIZE("write","vga dac read addr",len,1,1);
2421
2422     PASSTHROUGH_IO_OUT(vga,port,src,len);
2423
2424     // cannot be out of bounds since there are 256 regs
2425
2426     vga->vga_dac.vga_dac_read_addr =  *((uint8_t*)src) ;
2427
2428     // Now we also need to reset the state machine
2429
2430     vga->vga_dac.state=DAC_READ;
2431     vga->vga_dac.channel=RED;
2432
2433     return len;
2434 }
2435
2436
2437 static int dac_data_read(struct guest_info *core,
2438                          uint16_t port,
2439                          void *dest,
2440                          uint_t len,
2441                          void *priv_data)
2442 {
2443     struct vga_internal *vga = (struct vga_internal *) priv_data;
2444     uint8_t curreg;
2445     uint8_t curchannel;
2446     uint8_t data;
2447
2448     if (vga->vga_dac.state!=DAC_READ) {
2449         PrintError("vga: dac data read while in other state\n");
2450         // results undefined, so we continue
2451     }
2452
2453     ERR_WRONG_SIZE("read","vga dac read data",len,1,1);
2454
2455     curreg = vga->vga_dac.vga_dac_read_addr;
2456     curchannel = vga->vga_dac.channel;
2457     data = (vga->vga_dac.vga_dac_palette[curreg] >> curchannel*8) & 0x3f;
2458
2459     PrintDebug("vga: dac reg %u [%s] = 0x%x\n",
2460                curreg,
2461                curchannel == 0 ? "RED" : curchannel == 1 ? "GREEN"
2462                           : curchannel==2 ? "BLUE" : "BAD CHANNEL",
2463                data);
2464
2465     *((uint8_t*)dest) = data;
2466
2467     PASSTHROUGH_IO_IN(vga,port,dest,len);
2468
2469     PASSTHROUGH_READ_CHECK(vga,data,*(uint8_t*)dest);
2470
2471     curchannel = (curchannel+1)%3;
2472     vga->vga_dac.channel=curchannel;
2473     if (curchannel==0) {
2474         curreg = (curreg + 1) % VGA_DAC_NUM_ENTRIES;
2475     }
2476     vga->vga_dac.vga_dac_read_addr = curreg;
2477     vga->vga_dac.state=DAC_READ;
2478
2479     return len;
2480 }
2481
2482
2483
2484 static int dac_data_write(struct guest_info *core,
2485                           uint16_t port,
2486                           void *src,
2487                           uint_t len,
2488                           void *priv_data)
2489 {
2490     struct vga_internal *vga = (struct vga_internal *) priv_data;
2491     uint8_t curreg;
2492     uint8_t curchannel;
2493     uint8_t data;
2494     vga_palette_reg data32;
2495     vga_palette_reg mask32;
2496
2497     if (vga->vga_dac.state!=DAC_WRITE) {
2498         PrintError("vga: dac data write while in other state\n");
2499         // results undefined, so we continue
2500     }
2501
2502     ERR_WRONG_SIZE("read","vga dac write data",len,1,1);
2503
2504     PASSTHROUGH_IO_OUT(vga,port,src,len);
2505
2506     curreg = vga->vga_dac.vga_dac_write_addr;
2507     curchannel = vga->vga_dac.channel;
2508     data = *((uint8_t *)src);
2509
2510     PrintDebug("vga: dac reg %u [%s] write with 0x%x\n",
2511                curreg,
2512                curchannel == 0 ? "RED" : curchannel == 1 ? "GREEN"
2513                           : curchannel==2 ? "BLUE" : "BAD CHANNEL",
2514                data);
2515
2516     data32 = data & 0x3f ;
2517     data32 <<= curchannel*8;
2518     mask32 = ~(0xff << (curchannel * 8));
2519
2520     vga->vga_dac.vga_dac_palette[curreg] &= mask32;
2521     vga->vga_dac.vga_dac_palette[curreg] |= data32;
2522
2523     curchannel = (curchannel+1)%3;
2524     vga->vga_dac.channel=curchannel;
2525     if (curchannel==0) {
2526         curreg = (curreg + 1) % VGA_DAC_NUM_ENTRIES;
2527     }
2528     vga->vga_dac.vga_dac_write_addr = curreg;
2529     vga->vga_dac.state=DAC_WRITE;
2530
2531     render(vga);
2532
2533     return len;
2534 }
2535
2536
2537
2538 static int dac_pixel_mask_read(struct guest_info *core,
2539                                uint16_t port,
2540                                void *dest,
2541                                uint_t len,
2542                                void *priv_data)
2543 {
2544     struct vga_internal *vga = (struct vga_internal *) priv_data;
2545
2546     PrintDebug("vga: dac pixel mask read data=0x%x\n",
2547                vga->vga_dac.vga_pixel_mask);
2548
2549     ERR_WRONG_SIZE("read","vga pixel mask",len,1,1);
2550
2551     *((uint8_t*)dest) = vga->vga_dac.vga_pixel_mask;
2552
2553     PASSTHROUGH_IO_IN(vga,port,dest,len);
2554
2555     PASSTHROUGH_READ_CHECK(vga,vga->vga_dac.vga_pixel_mask,*(uint8_t*)dest);
2556
2557     return len;
2558 }
2559
2560 static int dac_pixel_mask_write(struct guest_info *core,
2561                                 uint16_t port,
2562                                 void *src,
2563                                 uint_t len,
2564                                 void *priv_data)
2565 {
2566     struct vga_internal *vga = (struct vga_internal *) priv_data;
2567     uint8_t new_data;
2568
2569     new_data=*((uint8_t*)src);
2570
2571     PrintDebug("vga: dac pixel mask write data=0x%x\n", new_data);
2572
2573     ERR_WRONG_SIZE("write","pixel mask",len,1,1);
2574
2575     PASSTHROUGH_IO_OUT(vga,port,src,len);
2576
2577     vga->vga_dac.vga_pixel_mask =  new_data;
2578
2579     return len;
2580 }
2581
2582 static int init_vga(struct vga_internal *vga)
2583 {
2584     // TODO: startup spec of register contents, if any
2585     PrintError("vga: init_vga is UNIMPLEMTED\n");
2586     return 0;
2587 }
2588
2589 static int free_vga(struct vga_internal *vga)
2590 {
2591     int i;
2592     int ret;
2593     struct vm_device *dev = vga->dev;
2594
2595     // Framebuffer deletion is user's responsibility
2596
2597     //    if (vga->mem_store) {
2598     //  V3_FreePages(v3_hva_to_hpa(vga->mem_store),MEM_REGION_NUM_PAGES);
2599     //  vga->mem_store=0;
2600     //}
2601
2602     for (i=0;i<MAP_NUM;i++) {
2603         if (vga->map[i]) {
2604             V3_FreePages(V3_PAddr(vga->map[i]),MAP_SIZE/4096);
2605             vga->map[i]=0;
2606         }
2607     }
2608
2609     v3_unhook_mem(vga->dev->vm, V3_MEM_CORE_ANY, MEM_REGION_START);
2610
2611     ret = 0;
2612
2613     ret |= v3_dev_unhook_io(dev, VGA_MISC_OUT_READ);
2614     // The following also covers VGA_INPUT_STAT0_READ
2615     ret |= v3_dev_unhook_io(dev, VGA_MISC_OUT_WRITE);
2616     // The following also covers VGA_FEATURE_CTRL_WRITE_MONO
2617     ret |= v3_dev_unhook_io(dev, VGA_INPUT_STAT1_READ_MONO);
2618     // The folowinn also covers VGA FEATURE_CONTROL_WRITE_COLOR
2619     ret |= v3_dev_unhook_io(dev, VGA_INPUT_STAT1_READ_COLOR);
2620     ret |= v3_dev_unhook_io(dev, VGA_FEATURE_CONTROL_READ);
2621
2622     ret |= v3_dev_unhook_io(dev, VGA_VIDEO_SUBSYS_ENABLE);
2623
2624     /* Sequencer registers */
2625     ret |= v3_dev_unhook_io(dev, VGA_SEQUENCER_ADDRESS);
2626     ret |= v3_dev_unhook_io(dev, VGA_SEQUENCER_DATA);
2627
2628     /* CRT controller registers */
2629     ret |= v3_dev_unhook_io(dev, VGA_CRT_CONTROLLER_ADDRESS_MONO);
2630     ret |= v3_dev_unhook_io(dev, VGA_CRT_CONTROLLER_ADDRESS_COLOR);
2631     ret |= v3_dev_unhook_io(dev, VGA_CRT_CONTROLLER_DATA_MONO);
2632     ret |= v3_dev_unhook_io(dev, VGA_CRT_CONTROLLER_DATA_COLOR);
2633
2634     /* graphics controller registers */
2635     ret |= v3_dev_unhook_io(dev, VGA_GRAPHICS_CONTROLLER_ADDRESS);
2636     ret |= v3_dev_unhook_io(dev, VGA_GRAPHICS_CONTROLLER_DATA);
2637
2638     /* attribute controller registers */
2639     ret |= v3_dev_unhook_io(dev, VGA_ATTRIBUTE_CONTROLLER_ADDRESS_AND_WRITE);
2640     ret |= v3_dev_unhook_io(dev, VGA_ATTRIBUTE_CONTROLLER_READ);
2641
2642     /* video DAC palette registers */
2643     ret |= v3_dev_unhook_io(dev, VGA_DAC_WRITE_ADDR);
2644     ret |= v3_dev_unhook_io(dev, VGA_DAC_READ_ADDR);
2645     ret |= v3_dev_unhook_io(dev, VGA_DAC_DATA);
2646     ret |= v3_dev_unhook_io(dev, VGA_DAC_PIXEL_MASK);
2647
2648
2649     if (vga->host_cons) {
2650         v3_graphics_console_close(vga->host_cons);
2651     }
2652
2653     V3_Free(vga);
2654
2655     return 0;
2656 }
2657
2658 static struct v3_device_ops dev_ops = {
2659     .free = (int (*)(void *))free_vga,
2660 };
2661
2662
2663 static int vga_init(struct v3_vm_info * vm, v3_cfg_tree_t * cfg) {
2664     struct vga_internal *vga;
2665     int i;
2666     int ret;
2667
2668     char * dev_id = v3_cfg_val(cfg, "ID");
2669     char * passthrough = v3_cfg_val(cfg, "passthrough");
2670     char * hostframebuf = v3_cfg_val(cfg, "hostframebuf");
2671
2672     PrintDebug("vga: init_device\n");
2673
2674     vga = (struct vga_internal *)V3_Malloc(sizeof(struct vga_internal));
2675
2676     if (!vga) {
2677         PrintError("vga: cannot allocate\n");
2678         return -1;
2679     }
2680
2681     memset(vga, 0, sizeof(struct vga_internal));
2682
2683     if (passthrough && strcasecmp(passthrough,"enable")==0) {
2684         PrintDebug("vga: enabling passthrough\n");
2685         vga->passthrough=true;
2686         vga->skip_next_passthrough_out=false;
2687     }
2688
2689
2690     if (hostframebuf && strcasecmp(hostframebuf,"enable")==0) {
2691         struct v3_frame_buffer_spec req;
2692
2693         PrintDebug("vga: enabling host frame buffer console (GRAPHICS_CONSOLE)\n");
2694
2695         memset(&req,0,sizeof(struct v3_frame_buffer_spec));
2696         memset(&(vga->target_spec),0,sizeof(struct v3_frame_buffer_spec));
2697
2698         req.height=VGA_MAXY;
2699         req.width=VGA_MAXX;
2700         req.bytes_per_pixel=4;
2701         req.bits_per_channel=8;
2702         req.red_offset=0;
2703         req.green_offset=1;
2704         req.blue_offset=2;
2705
2706
2707         vga->host_cons = v3_graphics_console_open(vm,&req,&(vga->target_spec));
2708
2709         if (!vga->host_cons) {
2710             PrintError("vga: unable to open host OS's graphics console\n");
2711             free_vga(vga);
2712             return -1;
2713         }
2714
2715         if (memcmp(&req,&(vga->target_spec),sizeof(req))) {
2716             PrintDebug("vga: warning: target spec differs from requested spec\n");
2717             PrintDebug("vga: request: %u by %u by %u with %u bpc and r,g,b at %u, %u, %u\n", req.width, req.height, req.bytes_per_pixel, req.bits_per_channel, req.red_offset, req.green_offset, req.blue_offset);
2718             PrintDebug("vga: response: %u by %u by %u with %u bpc and r,g,b at %u, %u, %u\n", vga->target_spec.width, vga->target_spec.height, vga->target_spec.bytes_per_pixel, vga->target_spec.bits_per_channel, vga->target_spec.red_offset, vga->target_spec.green_offset, vga->target_spec.blue_offset);
2719
2720         }
2721     }
2722
2723     if (!vga->passthrough && !vga->host_cons) {
2724         V3_Print("vga: neither passthrough nor host console are enabled - no way to display anything!\n");
2725     }
2726
2727
2728     // No memory store is allocated since we will use a full memory hook
2729     // The VGA maps can be read as well as written
2730     // Reads also affect writes, since they are how you fill the latches
2731
2732     // Now allocate the maps
2733     for (i=0;i<MAP_NUM;i++) {
2734         vga->map[i] = (vga_map) V3_VAddr((void*)V3_AllocPages(MAP_SIZE/4096));
2735         if (!(vga->map[i])) {
2736             PrintError("vga: cannot allocate maps\n");
2737             free_vga(vga);
2738             return -1;
2739         }
2740         memset(vga->map[i],0,MAP_SIZE);
2741     }
2742
2743     struct vm_device * dev = v3_add_device(vm, dev_id, &dev_ops, vga);
2744
2745     if (dev == NULL) {
2746         PrintError("Could not attach device %s\n", dev_id);
2747         free_vga(vga);
2748         return -1;
2749     }
2750
2751     vga->dev = dev;
2752
2753     if (v3_hook_full_mem(vm, V3_MEM_CORE_ANY,
2754                          MEM_REGION_START, MEM_REGION_END,
2755                          &vga_read,
2756                          &vga_write,
2757                          dev) == -1) {
2758         PrintError("vga: memory book failed\n");
2759         v3_remove_device(dev);
2760         return -1;
2761     }
2762
2763     ret = 0;
2764
2765     /* Miscelaneous registers */
2766     ret |= v3_dev_hook_io(dev, VGA_MISC_OUT_READ, &misc_out_read, NULL);
2767     // The following also covers VGA_INPUT_STAT0_READ
2768     ret |= v3_dev_hook_io(dev, VGA_MISC_OUT_WRITE, &input_stat0_read, &misc_out_write);
2769     // The following also covers VGA_FEATURE_CTRL_WRITE_MONO
2770     ret |= v3_dev_hook_io(dev, VGA_INPUT_STAT1_READ_MONO, &input_stat1_read, &feature_control_write);
2771     // The folowinn also covers VGA FEATURE_CONTROL_WRITE_COLOR
2772     ret |= v3_dev_hook_io(dev, VGA_INPUT_STAT1_READ_COLOR, &input_stat1_read, &feature_control_write);
2773     ret |= v3_dev_hook_io(dev, VGA_FEATURE_CONTROL_READ, &feature_control_read, NULL);
2774
2775     ret |= v3_dev_hook_io(dev, VGA_VIDEO_SUBSYS_ENABLE, &video_subsys_enable_read, &video_subsys_enable_write);
2776
2777     /* Sequencer registers */
2778     ret |= v3_dev_hook_io(dev, VGA_SEQUENCER_ADDRESS, &sequencer_address_read, &sequencer_address_write);
2779     ret |= v3_dev_hook_io(dev, VGA_SEQUENCER_DATA, &sequencer_data_read, &sequencer_data_write);
2780
2781     /* CRT controller registers */
2782     ret |= v3_dev_hook_io(dev, VGA_CRT_CONTROLLER_ADDRESS_MONO, &crt_controller_address_read,&crt_controller_address_write);
2783     ret |= v3_dev_hook_io(dev, VGA_CRT_CONTROLLER_ADDRESS_COLOR, &crt_controller_address_read,&crt_controller_address_write);
2784     ret |= v3_dev_hook_io(dev, VGA_CRT_CONTROLLER_DATA_MONO, &crt_controller_data_read,&crt_controller_data_write);
2785     ret |= v3_dev_hook_io(dev, VGA_CRT_CONTROLLER_DATA_COLOR, &crt_controller_data_read,&crt_controller_data_write);
2786
2787     /* graphics controller registers */
2788     ret |= v3_dev_hook_io(dev, VGA_GRAPHICS_CONTROLLER_ADDRESS, &graphics_controller_address_read,&graphics_controller_address_write);
2789     ret |= v3_dev_hook_io(dev, VGA_GRAPHICS_CONTROLLER_DATA, &graphics_controller_data_read,&graphics_controller_data_write);
2790
2791     /* attribute controller registers */
2792     ret |= v3_dev_hook_io(dev, VGA_ATTRIBUTE_CONTROLLER_ADDRESS_AND_WRITE, &attribute_controller_address_read,&attribute_controller_address_and_data_write);
2793     ret |= v3_dev_hook_io(dev, VGA_ATTRIBUTE_CONTROLLER_READ, &attribute_controller_data_read,NULL);
2794
2795     /* video DAC palette registers */
2796     ret |= v3_dev_hook_io(dev, VGA_DAC_WRITE_ADDR, &dac_write_address_read,&dac_write_address_write);
2797     ret |= v3_dev_hook_io(dev, VGA_DAC_READ_ADDR, &dac_read_address_read,&dac_read_address_write);
2798     ret |= v3_dev_hook_io(dev, VGA_DAC_DATA, &dac_data_read, &dac_data_write);
2799     ret |= v3_dev_hook_io(dev, VGA_DAC_PIXEL_MASK, &dac_pixel_mask_read, &dac_pixel_mask_write);
2800
2801     if (ret != 0) {
2802         PrintError("vga: Error allocating VGA I/O ports\n");
2803         v3_remove_device(dev);
2804         return -1;
2805     }
2806
2807     init_vga(vga);
2808
2809     PrintDebug("vga: successfully added and initialized.\n");
2810
2811     return 0;
2812
2813 }
2814
2815 device_register("VGA", vga_init);