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