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