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
  26 #include "vga_regs.h"
  27
  28 #define MEM_REGION_START 0xa0000
  29 #define MEM_REGION_END   0xc0000
  30 #define MEM_REGION_NUM_PAGES (((MEM_REGION_END)-(MEM_REGION_START))/4096)
  31
  32 #define MAP_SIZE 65536
  33 #define MAP_NUM  4
  34
  35 typedef uint8_t *vga_map; // points to MAP_SIZE data
  36
  37
  38 #define VGA_MISC_OUT_READ 0x3cc
  39 #define VGA_MISC_OUT_WRITE 0x3c2
  40
  41 #define VGA_INPUT_STAT0_READ 0x3c2
  42
  43 #define VGA_INPUT_STAT1_READ_MONO 0x3ba
  44 #define VGA_INPUT_STAT1_READ_COLOR 0x3da
  45
  46 #define VGA_FEATURE_CONTROL_READ 0x3ca
  47 #define VGA_FEATURE_CONTROL_WRITE_MONO 0x3ba
  48 #define VGA_FEATURE_CONTROL_WRITE_COLOR 0x3da
  49
  50 #define VGA_VIDEO_SUBSYS_ENABLE 0x3c3
  51
  52 #define VGA_SEQUENCER_ADDRESS 0x3c4
  53 #define VGA_SEQUENCER_DATA 0x3c5
  54 #define VGA_SEQUENCER_NUM 5
  55
  56
  57 #define VGA_CRT_CONTROLLER_ADDRESS_MONO 0x3b4
  58 #define VGA_CRT_CONTROLLER_ADDRESS_COLOR 0x3d4
  59 #define VGA_CRT_CONTROLLER_DATA_MONO 0x3b5
  60 #define VGA_CRT_CONTROLLER_DATA_COLOR 0x3d5
  61 #define VGA_CRT_CONTROLLER_NUM 25
  62
  63
  64 #define VGA_GRAPHICS_CONTROLLER_ADDRESS 0x3ce
  65 #define VGA_GRAPHICS_CONTROLLER_DATA 0x3cf
  66 #define VGA_GRAPHICS_CONTROLLER_NUM 9
  67
  68 #define VGA_ATTRIBUTE_CONTROLLER_ADDRESS_AND_WRITE 0x3c0
  69 #define VGA_ATTRIBUTE_CONTROLLER_READ 0x3c1
  70 #define VGA_ATTRIBUTE_CONTROLLER_NUM 21
  71
  72 #define VGA_DAC_WRITE_ADDR 0x3c8
  73 #define VGA_DAC_READ_ADDR 0x3c7
  74 #define VGA_DAC_DATA 0x3c8
  75 #define VGA_DAC_PIXEL_MASK 0x3c6
  76
  77 #define VGA_DAC_NUM_ENTRIES 256
  78
  79 struct vga_misc_regs {
  80     /* Read: 0x3cc; Write: 0x3c2 */
  81     struct vga_misc_out_reg        vga_misc_out;
  82     /* Read: 0x3c2 */
  83     struct vga_input_stat0_reg     vga_input_stat0;
  84     /* Read: 0x3?a  3ba for mono; 3da for cga set by misc.io_addr_sel */
  85     struct vga_input_stat1_reg     vga_input_stat1;
  86     /* Read: 0x3ca; Write: 0x3?a 3ba for mono 3da for color - set by misc.io_addr_sel*/
  87     struct vga_feature_control_reg vga_feature_control;
  88     /* Read: 0x3c3; Write: 0x3c3 */
  89     struct vga_video_subsys_enable_reg vga_video_subsys_enable;
  90 } __attribute__((packed));
  91
  92 struct vga_sequencer_regs {
  93     /*   Address register is 0x3c4, data register is 0x3c5 */
  94     /* 0x3c4 */
  95     struct vga_sequencer_addr_reg vga_sequencer_addr;
  96
  97     /* these can be accessed via the index, offset on start
  98        or via the specific regs.   For this reason, it is essential
  99        that this is all packed and that the order does not change */
 100
 101     uint8_t  vga_sequencer_regs[0];
 102
 103     /* Index 0 */
 104     struct vga_reset_reg   vga_reset;
 105     /* Index 1 */
 106     struct vga_clocking_mode_reg vga_clocking_mode;
 107     /* Index 2 */
 108     struct vga_map_mask_reg vga_map_mask;
 109     /* Index 3 */
 110     struct vga_char_map_select_reg vga_char_map_select;
 111     /* Index 4 */
 112     struct vga_mem_mode_reg  vga_mem_mode;
 113 } __attribute__((packed));
 114
 115 struct vga_crt_controller_regs {
 116     /* Address Register is 0x3b4 or 0x3d4 */
 117     /* Data register is 0x3b5 or 0x3d5 based on mono versus color */
 118     struct vga_crt_addr_reg vga_crt_addr;
 119
 120     /* these can be accessed via the index, offset on start
 121        or via the specific regs.   For this reason, it is essential
 122        that this is all packed and that the order does not change */
 123
 124     uint8_t  vga_crt_controller_regs[0];
 125
 126     /* index 0 */
 127     vga_horizontal_total_reg vga_horizontal_total;
 128     /* index 1 */
 129     vga_horizontal_display_enable_end_reg vga_horizontal_display_enable_end;
 130     /* index 2 */
 131     vga_start_horizontal_blanking_reg vga_start_horizontal_blanking;
 132     /* index 3 */
 133     struct vga_end_horizontal_blanking_reg vga_end_horizontal_blanking;
 134     /* index 4 */
 135     vga_start_horizontal_retrace_pulse_reg vga_start_horizontal_retrace_pulse;
 136     /* index 5 */
 137     struct vga_end_horizontal_retrace_reg vga_end_horizontal_retrace;
 138     /* index 6 */
 139     vga_vertical_total_reg vga_vertical_total;
 140     /* index 7 */
 141     struct vga_overflow_reg vga_overflow;
 142     /* index 8 */
 143     struct vga_preset_row_scan_reg vga_preset_row_scan;
 144     /* index 9 */
 145     struct vga_max_row_scan_reg vga_row_scan;
 146     /* index 10 */
 147     struct vga_cursor_start_reg vga_cursor_start;
 148     /* index 11 */
 149     struct vga_cursor_end_reg vga_cursor_end;
 150     /* index 12 */
 151     vga_start_address_high_reg vga_start_address_high;
 152     /* index 13 */
 153     vga_start_address_low_reg vga_start_address_low;
 154     /* index 14 */
 155     vga_cursor_location_high_reg vga_cursor_location_high;
 156     /* index 15 */
 157     vga_cursor_location_low_reg vga_cursor_location_low;
 158     /* index 16 */
 159     vga_vertical_retrace_start_reg vga_vertical_retrace_start;
 160     /* index 17 */
 161     struct vga_vertical_retrace_end_reg vga_vertical_retrace_end;
 162     /* index 18 */
 163     vga_vertical_display_enable_end_reg vga_vertical_display_enable;
 164     /* index 19 */
 165     vga_offset_reg vga_offset;
 166     /* index 20 */
 167     struct vga_underline_location_reg vga_underline_location;
 168     /* index 21 */
 169     vga_start_vertical_blanking_reg vga_start_vertical_blanking;
 170     /* index 22 */
 171     vga_end_vertical_blanking_reg vga_end_vertical_blanking;
 172     /* index 23 */
 173     struct vga_crt_mode_control_reg vga_crt_mode_control;
 174     /* index 24 */
 175     vga_line_compare_reg vga_line_compare;
 176 } __attribute__((packed));
 177
 178 struct vga_graphics_controller_regs {
 179     /*   Address: 0x3ce    Data: 0x3cf */
 180
 181     /* 0x3ce */
 182     struct vga_graphics_ctrl_addr_reg vga_graphics_ctrl_addr;
 183
 184     /* these can be accessed via the index, offset on start
 185        or via the specific regs.   For this reason, it is essential
 186        that this is all packed and that the order does not change */
 187
 188     uint8_t  vga_graphics_controller_regs[0];
 189
 190     /* Index 0 */
 191     struct vga_set_reset_reg vga_set_reset;
 192     /* Index 1 */
 193     struct vga_enable_set_reset_reg vga_enable_set_reset;
 194     /* Index 2 */
 195     struct vga_color_compare_reg vga_color_compare;
 196     /* Index 3 */
 197     struct vga_data_rotate_reg vga_data_rotate;
 198     /* Index 4 */
 199     struct vga_read_map_select_reg vga_read_map_select;
 200     /* Index 5 */
 201     struct vga_graphics_mode_reg vga_graphics_mode;
 202     /* Index 6 */
 203     struct vga_misc_reg vga_misc;
 204     /* Index 7 */
 205     struct vga_color_dont_care_reg vga_color_dont_care;
 206     /* Index 8 */
 207     vga_bit_mask_reg vga_bit_mask;
 208 } __attribute__((packed));
 209
 210
 211 struct vga_attribute_contoller_regs {
 212     /*
 213       Address AND WRITE: 0x3c0
 214       Read: 0x3c1
 215
 216       The write protocol is to write the index to 0x3c0 followed by
 217       the data.  The read protocol is to write the index to 0x3c0
 218       and then read from 0x3c1
 219
 220       IMPORTANT: write address, write data flips state back to write address
 221       write address, read data DOES NOT
 222
 223       To reset to write address state, read input status register 1
 224 */
 225     enum { ATTR_ADDR, ATTR_DATA }  state;  //state of the flip flop
 226
 227     /* 0x3c0 */
 228     struct vga_attribute_controller_address_reg vga_attribute_controller_addr;
 229
 230
 231
 232     /* these can be accessed via the index, offset on start
 233        or via the specific regs.   For this reason, it is essential
 234        that this is all packed and that the order does not change */
 235
 236     uint8_t  vga_attribute_controller_regs[0];
 237
 238     /* Indices 0..15 */
 239     vga_internal_palette_regs   vga_internal_palette;
 240     /* Index 16 */
 241     struct vga_attribute_mode_control_reg vga_attribute_mode_control;
 242     /* Index 17 */
 243     vga_overscan_color_reg vga_overscan_color;
 244     /* Index 18 */
 245     struct vga_color_plane_enable_reg vga_color_plane_enable;
 246     /* Index 19 */
 247     struct vga_horizontal_pixel_pan_reg vga_horizontal_pixel_pan;
 248     /* Index 20 */
 249     struct vga_color_select_reg vga_color_select;
 250 } __attribute__((packed));
 251
 252 struct vga_dac_regs {
 253     enum {DAC_READ=0, DAC_WRITE} state;
 254     enum {RED=0,GREEN,BLUE} channel;
 255     vga_dac_pixel_mask_reg vga_pixel_mask;
 256     vga_dac_write_addr_reg vga_dac_write_addr;
 257     vga_dac_read_addr_reg vga_dac_read_addr;
 258     // the dac_data register is used only to access the registers
 259     // and thus has no representation here
 260     vga_palette_reg vga_dac_palette[VGA_DAC_NUM_ENTRIES];
 261 } __attribute__((packed));
 262
 263
 264 struct vga_internal {
 265     struct vm_device *dev;
 266
 267     bool passthrough;
 268
 269     struct frame_buf *framebuf; // we render to this
 270
 271     //    void *mem_store;     // This is the region where the memory hooks will go
 272
 273     vga_map  map[MAP_NUM];  // the maps that the host writes to
 274
 275     uint8_t  latch[MAP_NUM];  // written to in any read, used during writes
 276
 277     /* Range of I/O ports here for backward compat with MDA and CGA */
 278     struct vga_misc_regs  vga_misc;
 279
 280     /* Address Register is 0x3b4 or 0x3d4 */
 281     /* Data register is 0x3b5 or 0x3d5 based on MDA/CGA/VGA (backward compat) */
 282     struct vga_crt_controller_regs vga_crt_controller;
 283
 284     /*   Address register is 0x3c4, data register is 0x3c5 */
 285     struct vga_sequencer_regs vga_sequencer;
 286
 287     /*   Address: 0x3ce    Data: 0x3cf */
 288     struct vga_graphics_controller_regs vga_graphics_controller;
 289
 290     /*
 291       Address AND WRITE: 0x3c0
 292       Read: 0x3c1
 293       Flip-Flop
 294     */
 295     struct vga_attribute_contoller_regs vga_attribute_controller;
 296
 297     /*
 298       address for reads: 0x3c7 (also resets state machine for access to 18 bit regs
 299       address for writes: 0x3c8 ("")
 300       data: 0x3c9
 301       pixel mask: 0x3c6 - do not write (init to 0xff)
 302     */
 303     struct vga_dac_regs vga_dac;
 304 };
 305
 306
 307
 308 static int vga_write(struct guest_info * core,
 309                      addr_t guest_addr,
 310                      void * src,
 311                      uint_t length,
 312                      void * priv_data)
 313 {
 314     struct vga_internal *vga = (struct vga_internal *) priv_data;
 315
 316     PrintDebug("vga: memory write: guest_addr=0x%p len=%u\n",(void*)guest_addr, length);
 317
 318     if (vga->passthrough) {
 319         memcpy(V3_VAddr((void*)guest_addr),src,length);
 320     }
 321
 322     /* Write mode determine by Graphics Mode Register (Index 05h).writemode */
 323
 324     switch (vga->vga_graphics_controller.vga_graphics_mode.write_mode) {
 325         case 0: {
 326
 327             /*
 328                00b -- Write Mode 0: In this mode, the host data is first rotated
 329                as per the Rotate Count field, then the Enable Set/Reset mechanism
 330                selects data from this or the Set/Reset field. Then the selected
 331                Logical Operation is performed on the resulting data and the data
 332                in the latch register. Then the Bit Mask field is used to select
 333                which bits come from the resulting data and which come
 334                from the latch register. Finally, only the bit planes enabled by
 335                the Memory Plane Write Enable field are written to memory.
 336             */
 337
 338             int i;
 339
 340             uint8_t  mapnum;
 341             uint64_t offset;
 342
 343             uint8_t ror = vga->vga_graphics_controller.vga_data_rotate.rotate_count;
 344             uint8_t func = vga->vga_graphics_controller.vga_data_rotate.function;
 345
 346             offset =(uint64_t) guest_addr - (uint64_t) MEM_REGION_START;
 347
 348             for (i=0;i<length;i++,offset++) {
 349                 // now for each map
 350                 uint8_t sr = vga->vga_graphics_controller.vga_set_reset.val & 0xf;
 351                 uint8_t esr = vga->vga_graphics_controller.vga_enable_set_reset.val &0xf;
 352                 uint8_t bm = vga->vga_graphics_controller.vga_bit_mask;
 353                 uint8_t mm = vga->vga_sequencer.vga_map_mask.val;
 354
 355                 for (mapnum=0;mapnum<4;mapnum++, sr>>=1, esr>>=1, bm>>=1, mm>>=1) {
 356                     vga_map map = vga->map[mapnum];
 357                     uint8_t data = ((uint8_t *)src)[i];
 358                     uint8_t latchval = vga->latch[mapnum];
 359
 360                     // rotate data right
 361                     data = (data>>ror) | data<<(8-ror);
 362
 363                     // use SR bit if ESR is on for this map
 364                     if (esr & 0x1) {
 365                         data = (uint8_t)((((sint8_t)(sr&0x1))<<7)>>7);  // expand sr bit
 366                     }
 367
 368                     // Apply function
 369                     switch (func) {
 370                         case 0: // NOP
 371                             break;
 372                         case 1: // AND
 373                             data &= latchval;
 374                             break;
 375                         case 2: // OR
 376                             data |= latchval;
 377                             break;
 378                         case 3: // XOR
 379                             data ^= latchval;
 380                             break;
 381                     }
 382
 383                     // mux between latch and alu output
 384                     if (bm & 0x1) {
 385                         // use alu output, which is in data
 386                     } else {
 387                         // use latch value
 388                         data=latchval;
 389                     }
 390
 391                     // selective write
 392                     if (mm & 0x1) {
 393                         // write to this map
 394                         map[offset] = data;
 395                     } else {
 396                         // skip this map
 397                     }
 398                 }
 399             }
 400         }
 401             break;
 402
 403
 404
 405         case 1: {
 406             /*
 407                01b -- Write Mode 1: In this mode, data is transferred directly
 408                from the 32 bit latch register to display memory, affected only by
 409                the Memory Plane Write Enable field. The host data is not used in this mode.
 410             */
 411
 412             int i;
 413
 414             uint64_t offset =(uint64_t) guest_addr - (uint64_t) MEM_REGION_START;
 415
 416             for (i=0;i<length;i++,offset++) {
 417
 418                 uint8_t mapnum;
 419                 uint8_t mm = vga->vga_sequencer.vga_map_mask.val;
 420
 421                 for (mapnum=0;mapnum<4;mapnum++,  mm>>=1) {
 422                     vga_map map = vga->map[mapnum];
 423                     uint8_t latchval = vga->latch[mapnum];
 424
 425                     // selective write
 426                     if (mm & 0x1) {
 427                         // write to this map
 428                         map[offset] = latchval;
 429                     } else {
 430                         // skip this map
 431                     }
 432                 }
 433             }
 434         }
 435             break;
 436
 437         case 2: {
 438             /*
 439               10b -- Write Mode 2: In this mode, the bits 3-0 of the host data
 440               are replicated across all 8 bits of their respective planes.
 441               Then the selected Logical Operation is performed on the resulting
 442               data and the data in the latch register. Then the Bit Mask field is used to
 443               select which bits come from the resulting data and which come from
 444               the latch register. Finally, only the bit planes enabled by the
 445               Memory Plane Write Enable field are written to memory.
 446             */
 447             int i;
 448             uint8_t  mapnum;
 449             uint64_t offset;
 450
 451             uint8_t func = vga->vga_graphics_controller.vga_data_rotate.function;
 452
 453             offset =(uint64_t) guest_addr - (uint64_t) MEM_REGION_START;
 454
 455             for (i=0;i<length;i++,offset++) {
 456                 // now for each map
 457                 uint8_t bm = vga->vga_graphics_controller.vga_bit_mask;
 458                 uint8_t mm = vga->vga_sequencer.vga_map_mask.val;
 459
 460                 for (mapnum=0;mapnum<4;mapnum++,  bm>>=1, mm>>=1) {
 461                     vga_map map = vga->map[mapnum];
 462                     uint8_t data = ((uint8_t *)src)[i];
 463                     uint8_t latchval = vga->latch[mapnum];
 464
 465                     // expand relevant bit to 8 bit
 466                     // it's basically esr=1, sr=bit from write
 467                     data = (uint8_t)(((sint8_t)(((data>>mapnum)&0x1)<<7))>>7);
 468
 469                     // Apply function
 470                     switch (func) {
 471                         case 0: // NOP
 472                             break;
 473                         case 1: // AND
 474                             data &= latchval;
 475                             break;
 476                         case 2: // OR
 477                             data |= latchval;
 478                             break;
 479                         case 3: // XOR
 480                             data ^= latchval;
 481                             break;
 482                     }
 483
 484                     // mux between latch and alu output
 485                     if (bm & 0x1) {
 486                         // use alu output, which is in data
 487                     } else {
 488                         // use latch value
 489                         data=latchval;
 490                     }
 491
 492                     // selective write
 493                     if (mm & 0x1) {
 494                         // write to this map
 495                         map[offset] = data;
 496                     } else {
 497                         // skip this map
 498                     }
 499                 }
 500             }
 501         }
 502             break;
 503
 504         case 3: {
 505             /* 11b -- Write Mode 3: In this mode, the data in the Set/Reset field is used
 506                as if the Enable Set/Reset field were set to 1111b. Then the host data is
 507                first rotated as per the Rotate Count field, then logical ANDed with the
 508                value of the Bit Mask field. The resulting value is used on the data
 509                obtained from the Set/Reset field in the same way that the Bit Mask field
 510                would ordinarily be used. to select which bits come from the expansion
 511                of the Set/Reset field and which come from the latch register. Finally,
 512                only the bit planes enabled by the Memory Plane Write Enable field
 513                are written to memory.
 514             */
 515             int i;
 516
 517             uint8_t  mapnum;
 518             uint64_t offset;
 519
 520             uint8_t ror = vga->vga_graphics_controller.vga_data_rotate.rotate_count;
 521
 522             offset =(uint64_t) guest_addr - (uint64_t) MEM_REGION_START;
 523
 524             for (i=0;i<length;i++,offset++) {
 525                 // now for each map
 526                 uint8_t data = ((uint8_t *)src)[i];
 527
 528                 data = (data>>ror) | data<<(8-ror);
 529
 530                 uint8_t bm = vga->vga_graphics_controller.vga_bit_mask & data;
 531                 uint8_t sr = vga->vga_graphics_controller.vga_set_reset.val & 0xf;
 532                 uint8_t mm = vga->vga_sequencer.vga_map_mask.val;
 533
 534                 for (mapnum=0;mapnum<4;mapnum++, sr>>=1, bm>>=1, mm>>=1) {
 535                     vga_map map = vga->map[mapnum];
 536                     uint8_t latchval = vga->latch[mapnum];
 537
 538                     data = (uint8_t)((((sint8_t)(sr&0x1))<<7)>>7);  // expand sr bit
 539
 540
 541                     // mux between latch and alu output
 542                     if (bm & 0x1) {
 543                         // use alu output, which is in data
 544                     } else {
 545                         // use latch value
 546                         data=latchval;
 547                     }
 548
 549                     // selective write
 550                     if (mm & 0x1) {
 551                         // write to this map
 552                         map[offset] = data;
 553                     } else {
 554                         // skip this map
 555                     }
 556                 }
 557             }
 558
 559         }
 560             break;
 561
 562             // There is no default
 563     }
 564
 565     return length;
 566 }
 567
 568     //    PrintError("vga: vga_write UNIMPLEMENTED\n");
 569
 570 /*
 571 up to 256K mapped through a window of 128K
 572
 573 most cards support linear mode as well
 574
 575 Need to implement readability too
 576
 577 Write extended memory bit to enable all 256K:
 578
 579    Sequencer Memory Mode Register (Index 04h) . extended memory
 580
 581 Must enable writes before effects happen:
 582
 583   Miscellaneous Output Register (Read at 3CCh, Write at 3C2h).ram enable
 584
 585 Choose which addresses are supported for CPU writes:
 586
 587 Miscellaneous Graphics Register (Index 06h).memory map select
 588
 589 00b -- A0000h-BFFFFh (128K region)
 590 01b -- A0000h-AFFFFh (64K region)
 591 10b -- B0000h-B7FFFh (32K region)
 592 11b -- B8000h-BFFFFh (32K region)
 593
 594 There are three addressing modes: Chain 4, Odd/Even mode, and normal mode:
 595
 596 Chain 4: This mode is used for MCGA emulation in the 320x200 256-color mode. The address is mapped to memory MOD 4 (shifted right 2 places.)
 597
 598 Memory model: 64K 32 bit locations; divided into 4 64K bit planes
 599
 600
 601
 602
 603
 604 Assume linear framebuffer, starting at address buf:
 605
 606 */
 607
 608
 609
 610 static int vga_read(struct guest_info * core,
 611                     addr_t guest_addr,
 612                     void * dst,
 613                     uint_t length,
 614                     void * priv_data)
 615 {
 616
 617     struct vga_internal *vga = (struct vga_internal *) priv_data;
 618
 619     PrintDebug("vga: memory read: guest_addr=0x%p len=%u\n",(void*)guest_addr, length);
 620
 621     if (vga->passthrough) {
 622         memcpy(dst,V3_VAddr((void*)guest_addr),length);
 623     }
 624
 625     /*
 626       Reading, 2 modes, set via Graphics Mode Register (index 05h).Read Mode:
 627     */
 628     switch (vga->vga_graphics_controller.vga_graphics_mode.read_mode) {
 629         case 0: {
 630             /*      0 - a byte from ONE of the 4 planes is returned;
 631                     which plane is determined by Read Map Select (Read Map Select Register (Index 04h)) */
 632             uint8_t  mapnum;
 633             uint64_t offset;
 634
 635             mapnum = vga->vga_graphics_controller.vga_read_map_select.map_select;
 636             offset =(uint64_t) guest_addr - (uint64_t) MEM_REGION_START;
 637
 638             if (offset>=65536) {
 639                 PrintError("vga: read to offset=%llu map=%u (%u bytes)\n",offset,mapnum,length);
 640             }
 641
 642             memcpy(dst,(vga->map[mapnum])+offset,length);
 643
 644             // load the latches with the last item read
 645             for (mapnum=0;mapnum<4;mapnum++) {
 646                 vga->latch[mapnum] = vga->map[mapnum][offset+length-1];
 647             }
 648
 649
 650         }
 651             break;
 652         case 1: {
 653             /*      1 - Compare video memory and reference color
 654                     (in Color Compare, except those not set in Color Don't Care),
 655                     each bit in returned result is one comparison between the reference color
 656
 657                     Ref color is *4* bits, and thus a one byte read returns a comparison
 658                     with 8 pixels
 659
 660             */
 661             int i;
 662
 663             uint8_t cc=vga->vga_graphics_controller.vga_color_compare.val & 0xf ;
 664             uint8_t dc=vga->vga_graphics_controller.vga_color_dont_care.val & 0xf;
 665
 666             uint8_t  mapnum;
 667             uint64_t offset;
 668             uint8_t  byte;
 669             uint8_t  bits;
 670
 671             offset =(uint64_t) guest_addr - (uint64_t) MEM_REGION_START;
 672
 673             for (i=0;i<length;i++,offset++) {
 674                 vga_map map;
 675                 byte=0;
 676                 for (mapnum=0;mapnum<4;mapnum++) {
 677                     map = vga->map[mapnum];
 678                     if ( (dc>>mapnum)&0x1 ) { // don't care
 679                         bits=0;
 680                     } else {
 681                         // lower 4 bits
 682                         bits = (map[offset]&0xf) == cc;
 683                         bits <<= 1;
 684                         // upper 4 bits
 685                         bits |= (((map[offset]>>4))&0xf) == cc;
 686                     }
 687                     // not clear whether it is 0..k or k..0
 688                     byte<<=2;
 689                     byte|=bits;
 690                 }
 691             }
 692
 693             // load the latches with the last item read
 694             for (mapnum=0;mapnum<4;mapnum++) {
 695                 vga->latch[mapnum] = vga->map[mapnum][offset+length-1];
 696             }
 697
 698         }
 699             break;
 700             // there is no default
 701     }
 702
 703     return length;
 704
 705 }
 706
 707
 708
 709 static int render(struct vga_internal *vga)
 710 {
 711     PrintError("vga: render UNIMPLEMENTED\n");
 712     return 0;
 713 }
 714
 715
 716 #define ERR_WRONG_SIZE(op,reg,len,min,max)      \
 717     if (((len)<(min)) || ((len)>(max))) {       \
 718         PrintError("vga: %s of %s wrong size (%d bytes, but only %d to %d allowed)\n",(op),(reg),(len),(min),(max)); \
 719         return -1; \
 720 }
 721
 722 static inline void passthrough_io_in(uint16_t port, void * dest, uint_t length) {
 723     switch (length) {
 724         case 1:
 725             *(uint8_t *)dest = v3_inb(port);
 726             break;
 727         case 2:
 728             *(uint16_t *)dest = v3_inw(port);
 729             break;
 730         case 4:
 731             *(uint32_t *)dest = v3_indw(port);
 732             break;
 733         default:
 734             PrintError("vga: unsupported passthrough io in size %u\n",length);
 735             break;
 736     }
 737 }
 738
 739
 740 static inline void passthrough_io_out(uint16_t port, const void * src, uint_t length) {
 741     switch (length) {
 742         case 1:
 743             v3_outb(port, *(uint8_t *)src);
 744             break;
 745         case 2:
 746             v3_outw(port, *(uint16_t *)src);
 747             break;
 748         case 4:
 749             v3_outdw(port, *(uint32_t *)src);
 750             break;
 751         default:
 752             PrintError("vga: unsupported passthrough io out size %u\n",length);
 753             break;
 754     }
 755 }
 756
 757 #define PASSTHROUGH_IO_IN(vga,port,dest,len)                            \
 758     do { if ((vga)->passthrough) { passthrough_io_in(port,dest,len); } } while (0)
 759
 760 #define PASSTHROUGH_IO_OUT(vga,port,src,len)                            \
 761     do { if ((vga)->passthrough) { passthrough_io_in(port,src,len); } } while (0)
 762
 763
 764
 765 static int misc_out_read(struct guest_info *core,
 766                          uint16_t port,
 767                          void *dest,
 768                          uint_t len,
 769                          void *priv_data)
 770 {
 771     struct vga_internal *vga = (struct vga_internal *) priv_data;
 772
 773     PrintDebug("vga: misc out read data=0x%x\n", vga->vga_misc.vga_misc_out.val);
 774
 775     ERR_WRONG_SIZE("read","misc out",len,1,1);
 776
 777     PASSTHROUGH_IO_IN(vga,port,dest,len);
 778
 779     *((uint8_t*)dest) = vga->vga_misc.vga_misc_out.val;
 780
 781     return len;
 782 }
 783
 784 static int misc_out_write(struct guest_info *core,
 785                           uint16_t port,
 786                           void *src,
 787                           uint_t len,
 788                           void *priv_data)
 789 {
 790     struct vga_internal *vga = (struct vga_internal *) priv_data;
 791
 792     PrintDebug("vga: misc out write data=0x%x\n", *((uint8_t*)src));
 793
 794     ERR_WRONG_SIZE("write","misc out",len,1,1);
 795
 796     PASSTHROUGH_IO_OUT(vga,port,src,len);
 797
 798     vga->vga_misc.vga_misc_out.val =  *((uint8_t*)src) ;
 799
 800     render(vga);
 801
 802     return len;
 803 }
 804
 805
 806
 807 static int input_stat0_read(struct guest_info *core,
 808                             uint16_t port,
 809                             void *dest,
 810                             uint_t len,
 811                             void *priv_data)
 812 {
 813     struct vga_internal *vga = (struct vga_internal *) priv_data;
 814
 815     PrintDebug("vga: input stat0  read data=0x%x\n", vga->vga_misc.vga_input_stat0.val);
 816
 817     ERR_WRONG_SIZE("read","input stat0",len,1,1);
 818
 819     PASSTHROUGH_IO_IN(vga,port,dest,len);
 820
 821     *((uint8_t*)dest) = vga->vga_misc.vga_input_stat0.val;
 822
 823     return len;
 824 }
 825
 826
 827 static int input_stat1_read(struct guest_info *core,
 828                             uint16_t port,
 829                             void *dest,
 830                             uint_t len,
 831                             void *priv_data)
 832 {
 833     struct vga_internal *vga = (struct vga_internal *) priv_data;
 834
 835     PrintDebug("vga: input stat0 (%s) read data=0x%x\n",
 836                port==0x3ba ? "mono" : "color",
 837                vga->vga_misc.vga_input_stat1.val);
 838
 839     ERR_WRONG_SIZE("read","input stat1",len,1,1);
 840
 841     PASSTHROUGH_IO_IN(vga,port,dest,len);
 842
 843     *((uint8_t*)dest) = vga->vga_misc.vga_input_stat1.val;
 844
 845     // Stunningly, reading stat1 is also a way to reset
 846     // the state of attribute controller address/data flipflop
 847     // That is some mighty fine crack the designers were smoking.
 848
 849     vga->vga_attribute_controller.state=ATTR_ADDR;
 850
 851     return len;
 852 }
 853
 854
 855 static int feature_control_read(struct guest_info *core,
 856                                 uint16_t port,
 857                                 void *dest,
 858                                 uint_t len,
 859                                 void *priv_data)
 860 {
 861     struct vga_internal *vga = (struct vga_internal *) priv_data;
 862
 863     PrintDebug("vga: feature control  read data=0x%x\n",
 864                vga->vga_misc.vga_feature_control.val);
 865
 866     ERR_WRONG_SIZE("read","feature control",len,1,1);
 867
 868     PASSTHROUGH_IO_IN(vga,port,dest,len);
 869
 870     *((uint8_t*)dest) = vga->vga_misc.vga_feature_control.val;
 871
 872     return len;
 873 }
 874
 875 static int feature_control_write(struct guest_info *core,
 876                                  uint16_t port,
 877                                  void *src,
 878                                  uint_t len,
 879                                  void *priv_data)
 880 {
 881     struct vga_internal *vga = (struct vga_internal *) priv_data;
 882
 883     PrintDebug("vga: feature control (%s) write data=0x%x\n",
 884                port==0x3ba ? "mono" : "color",
 885                *((uint8_t*)src));
 886
 887     ERR_WRONG_SIZE("write","feature control",len,1,1);
 888
 889     PASSTHROUGH_IO_OUT(vga,port,src,len);
 890
 891     vga->vga_misc.vga_feature_control.val =  *((uint8_t*)src) ;
 892
 893     render(vga);
 894
 895     return len;
 896 }
 897
 898
 899 static int video_subsys_enable_read(struct guest_info *core,
 900                                     uint16_t port,
 901                                     void *dest,
 902                                     uint_t len,
 903                                     void *priv_data)
 904 {
 905     struct vga_internal *vga = (struct vga_internal *) priv_data;
 906
 907     PrintDebug("vga: video subsys enable read data=0x%x\n",
 908                vga->vga_misc.vga_video_subsys_enable.val);
 909
 910     ERR_WRONG_SIZE("read","video subsys enable",len,1,1);
 911
 912     PASSTHROUGH_IO_IN(vga,port,dest,len);
 913
 914     *((uint8_t*)dest) = vga->vga_misc.vga_video_subsys_enable.val;
 915
 916     return len;
 917 }
 918
 919 static int video_subsys_enable_write(struct guest_info *core,
 920                                      uint16_t port,
 921                                      void *src,
 922                                      uint_t len,
 923                                      void *priv_data)
 924 {
 925     struct vga_internal *vga = (struct vga_internal *) priv_data;
 926
 927     PrintDebug("vga: video subsys enable write data=0x%x\n", *((uint8_t*)src));
 928
 929     ERR_WRONG_SIZE("write","video subsys enable",len,1,1);
 930
 931     PASSTHROUGH_IO_OUT(vga,port,src,len);
 932
 933     vga->vga_misc.vga_video_subsys_enable.val =  *((uint8_t*)src) ;
 934
 935     render(vga);
 936
 937     return len;
 938 }
 939
 940 static int sequencer_address_read(struct guest_info *core,
 941                                   uint16_t port,
 942                                   void *dest,
 943                                   uint_t len,
 944                                   void *priv_data)
 945 {
 946     struct vga_internal *vga = (struct vga_internal *) priv_data;
 947
 948     PrintDebug("vga: sequencer address read data=0x%x\n",
 949                vga->vga_sequencer.vga_sequencer_addr.val);
 950
 951     ERR_WRONG_SIZE("read","vga sequencer addr",len,1,1);
 952
 953     PASSTHROUGH_IO_IN(vga,port,dest,len);
 954
 955     *((uint8_t*)dest) = vga->vga_sequencer.vga_sequencer_addr.val;
 956
 957     return len;
 958 }
 959
 960 static int sequencer_address_write(struct guest_info *core,
 961                                   uint16_t port,
 962                                   void *src,
 963                                   uint_t len,
 964                                   void *priv_data)
 965 {
 966     struct vga_internal *vga = (struct vga_internal *) priv_data;
 967     uint8_t new_addr;
 968
 969     new_addr=*((uint8_t*)src);
 970
 971     PrintDebug("vga: sequencer address write data=0x%x\n", new_addr);
 972
 973     ERR_WRONG_SIZE("write","vga sequenver addr",len,1,1);
 974
 975     PASSTHROUGH_IO_OUT(vga,port,src,len);
 976
 977     if (new_addr>VGA_SEQUENCER_NUM) {
 978         PrintError("vga: ignoring change of sequencer address to %u (>%u)\n",
 979                    new_addr, VGA_SEQUENCER_NUM);
 980         //return -1;
 981     } else {
 982         vga->vga_sequencer.vga_sequencer_addr.val =  *((uint8_t*)src) ;
 983     }
 984
 985     return len;
 986 }
 987
 988 static int sequencer_data_read(struct guest_info *core,
 989                               uint16_t port,
 990                               void *dest,
 991                               uint_t len,
 992                               void *priv_data)
 993 {
 994     struct vga_internal *vga = (struct vga_internal *) priv_data;
 995     uint8_t index;
 996     uint8_t data;
 997
 998     index=vga->vga_sequencer.vga_sequencer_addr.val;  // should mask probably
 999     data=vga->vga_sequencer.vga_sequencer_regs[index];
1000
1001     PrintDebug("vga: sequencer data read data (index=%d) = 0x%x\n",
1002                index, data);
1003
1004     ERR_WRONG_SIZE("read","vga sequenver data",len,1,1);
1005
1006     PASSTHROUGH_IO_IN(vga,port,dest,len);
1007
1008     *((uint8_t*)dest) = data;
1009
1010     return len;
1011 }
1012
1013 static int sequencer_data_write(struct guest_info *core,
1014                                uint16_t port,
1015                                void *src,
1016                                uint_t len,
1017                                void *priv_data)
1018 {
1019     struct vga_internal *vga = (struct vga_internal *) priv_data;
1020     uint8_t index;
1021     uint8_t data;
1022
1023     data=*((uint8_t*)src);
1024     index=vga->vga_sequencer.vga_sequencer_addr.val;  // should mask probably
1025
1026     PrintDebug("vga: sequencer write data (index=%d) with 0x%x\n",
1027                index, data);
1028
1029     ERR_WRONG_SIZE("write","vga sequencer data",len,1,1);
1030
1031     PASSTHROUGH_IO_OUT(vga,port,src,len);
1032
1033     vga->vga_sequencer.vga_sequencer_regs[index] = data;
1034
1035     render(vga);
1036
1037     return len;
1038 }
1039
1040
1041
1042
1043 static int crt_controller_address_read(struct guest_info *core,
1044                                         uint16_t port,
1045                                         void *dest,
1046                                         uint_t len,
1047                                         void *priv_data)
1048 {
1049     struct vga_internal *vga = (struct vga_internal *) priv_data;
1050
1051     PrintDebug("vga: crt controller (%s) address read data=0x%x\n",
1052                port==0x3b4 ? "mono" : "color",
1053                vga->vga_crt_controller.vga_crt_addr.val);
1054
1055     ERR_WRONG_SIZE("read","vga crt controller addr",len,1,1);
1056
1057     PASSTHROUGH_IO_IN(vga,port,dest,len);
1058
1059     *((uint8_t*)dest) = vga->vga_crt_controller.vga_crt_addr.val;
1060
1061     return len;
1062 }
1063
1064 static int crt_controller_address_write(struct guest_info *core,
1065                                         uint16_t port,
1066                                         void *src,
1067                                         uint_t len,
1068                                         void *priv_data)
1069 {
1070     struct vga_internal *vga = (struct vga_internal *) priv_data;
1071     uint8_t new_addr;
1072
1073     new_addr=*((uint8_t*)src);
1074
1075     PrintDebug("vga: crt controller (%s) address write data=0x%x\n",
1076                port==0x3b4 ? "mono" : "color",
1077                new_addr);
1078
1079     ERR_WRONG_SIZE("write","vga crt controller addr",len,1,1);
1080
1081     PASSTHROUGH_IO_OUT(vga,port,src,len);
1082
1083     if (new_addr>VGA_CRT_CONTROLLER_NUM) {
1084         PrintError("vga: ignoring change of crt controller address to %u (>%u)\n",
1085                    new_addr, VGA_CRT_CONTROLLER_NUM);
1086         //return -1;
1087     } else {
1088         vga->vga_crt_controller.vga_crt_addr.val =  *((uint8_t*)src) ;
1089     }
1090
1091     return len;
1092 }
1093
1094 static int crt_controller_data_read(struct guest_info *core,
1095                                     uint16_t port,
1096                                     void *dest,
1097                                     uint_t len,
1098                                     void *priv_data)
1099 {
1100     struct vga_internal *vga = (struct vga_internal *) priv_data;
1101     uint8_t index;
1102     uint8_t data;
1103
1104     index=vga->vga_crt_controller.vga_crt_addr.val;  // should mask probably
1105     data=vga->vga_crt_controller.vga_crt_controller_regs[index];
1106
1107     PrintDebug("vga: crt controller data (%s) read data (index=%d) = 0x%x\n",
1108                port==0x3b5 ? "mono" : "color",
1109                index, data);
1110
1111     ERR_WRONG_SIZE("read","vga crt controller data",len,1,1);
1112
1113     PASSTHROUGH_IO_IN(vga,port,dest,len);
1114
1115     *((uint8_t*)dest) = data;
1116
1117     return len;
1118 }
1119
1120 static int crt_controller_data_write(struct guest_info *core,
1121                                      uint16_t port,
1122                                      void *src,
1123                                      uint_t len,
1124                                      void *priv_data)
1125 {
1126     struct vga_internal *vga = (struct vga_internal *) priv_data;
1127     uint8_t index;
1128     uint8_t data;
1129
1130     data=*((uint8_t*)src);
1131
1132     index=vga->vga_crt_controller.vga_crt_addr.val;  // should mask probably
1133
1134     PrintDebug("vga: crt controller (%s) write data (index=%d) with 0x%x\n",
1135                port==0x3b5 ? "mono" : "color",
1136                index, data);
1137
1138     ERR_WRONG_SIZE("write","vga crt controller data",len,1,1);
1139
1140     PASSTHROUGH_IO_OUT(vga,port,src,len);
1141
1142     vga->vga_crt_controller.vga_crt_controller_regs[index] = data;
1143
1144     render(vga);
1145
1146     return len;
1147 }
1148
1149
1150 static int graphics_controller_address_read(struct guest_info *core,
1151                                             uint16_t port,
1152                                             void *dest,
1153                                             uint_t len,
1154                                             void *priv_data)
1155 {
1156     struct vga_internal *vga = (struct vga_internal *) priv_data;
1157
1158     PrintDebug("vga: graphics controller address read data=0x%x\n",
1159                vga->vga_graphics_controller.vga_graphics_ctrl_addr.val);
1160
1161     ERR_WRONG_SIZE("read","vga graphics controller addr",len,1,1);
1162
1163     PASSTHROUGH_IO_IN(vga,port,dest,len);
1164
1165     *((uint8_t*)dest) = vga->vga_graphics_controller.vga_graphics_ctrl_addr.val;
1166
1167     return len;
1168 }
1169
1170 static int graphics_controller_address_write(struct guest_info *core,
1171                                              uint16_t port,
1172                                              void *src,
1173                                              uint_t len,
1174                                              void *priv_data)
1175 {
1176     struct vga_internal *vga = (struct vga_internal *) priv_data;
1177     uint8_t new_addr;
1178
1179     new_addr=*((uint8_t*)src);
1180
1181     PrintDebug("vga: graphics controller address write data=0x%x\n", new_addr);
1182
1183     ERR_WRONG_SIZE("write","vga graphics controller addr",len,1,1);
1184
1185     PASSTHROUGH_IO_OUT(vga,port,src,len);
1186
1187     if (new_addr>VGA_GRAPHICS_CONTROLLER_NUM) {
1188         PrintError("vga: ignoring change of graphics controller address to %u (>%u)\n",
1189                    new_addr, VGA_GRAPHICS_CONTROLLER_NUM);
1190         //return -1;
1191     } else {
1192         vga->vga_graphics_controller.vga_graphics_ctrl_addr.val =  *((uint8_t*)src) ;
1193     }
1194
1195     return len;
1196 }
1197
1198 static int graphics_controller_data_read(struct guest_info *core,
1199                                          uint16_t port,
1200                                          void *dest,
1201                                          uint_t len,
1202                                          void *priv_data)
1203 {
1204     struct vga_internal *vga = (struct vga_internal *) priv_data;
1205     uint8_t index;
1206     uint8_t data;
1207
1208     index=vga->vga_graphics_controller.vga_graphics_ctrl_addr.val;  // should mask probably
1209     data=vga->vga_graphics_controller.vga_graphics_controller_regs[index];
1210
1211     PrintDebug("vga: graphics controller data read data (index=%d) = 0x%x\n",
1212                index, data);
1213
1214     ERR_WRONG_SIZE("read","vga graphics controller data",len,1,1);
1215
1216     PASSTHROUGH_IO_IN(vga,port,dest,len);
1217
1218     *((uint8_t*)dest) = data;
1219
1220     return len;
1221 }
1222
1223 static int graphics_controller_data_write(struct guest_info *core,
1224                                           uint16_t port,
1225                                           void *src,
1226                                           uint_t len,
1227                                           void *priv_data)
1228 {
1229     struct vga_internal *vga = (struct vga_internal *) priv_data;
1230     uint8_t index;
1231     uint8_t data;
1232
1233     data=*((uint8_t*)src);
1234     index=vga->vga_graphics_controller.vga_graphics_ctrl_addr.val;  // should mask probably
1235
1236     PrintDebug("vga: graphics_controller write data (index=%d) with 0x%x\n",
1237                index, data);
1238
1239     ERR_WRONG_SIZE("write","vga graphics controller data",len,1,1);
1240
1241     PASSTHROUGH_IO_OUT(vga,port,src,len);
1242
1243     vga->vga_graphics_controller.vga_graphics_controller_regs[index] = data;
1244
1245     render(vga);
1246
1247     return len;
1248 }
1249
1250
1251
1252 /* Note that these guys have a bizarre protocol*/
1253
1254 static int attribute_controller_address_read(struct guest_info *core,
1255                                              uint16_t port,
1256                                              void *dest,
1257                                              uint_t len,
1258                                              void *priv_data)
1259 {
1260     struct vga_internal *vga = (struct vga_internal *) priv_data;
1261
1262     PrintDebug("vga: attribute controller address read data=0x%x\n",
1263                vga->vga_attribute_controller.vga_attribute_controller_addr.val);
1264
1265     ERR_WRONG_SIZE("read","vga attribute  controller addr",len,1,1);
1266
1267     PASSTHROUGH_IO_IN(vga,port,dest,len);
1268
1269     *((uint8_t*)dest) = vga->vga_attribute_controller.vga_attribute_controller_addr.val;
1270
1271     // Reading the attribute controller does not change the state
1272
1273     return len;
1274 }
1275
1276 static int attribute_controller_address_and_data_write(struct guest_info *core,
1277                                                        uint16_t port,
1278                                                        void *src,
1279                                                        uint_t len,
1280                                                        void *priv_data)
1281 {
1282     struct vga_internal *vga = (struct vga_internal *) priv_data;
1283
1284
1285     if (vga->vga_attribute_controller.state==ATTR_ADDR) {
1286         uint8_t new_addr = *((uint8_t*)src);
1287         // We are to treat this as an address write, and flip state
1288         // to expect data ON THIS SAME PORT
1289         PrintDebug("vga: attribute controller address write data=0x%x\n", new_addr);
1290
1291         ERR_WRONG_SIZE("write","vga attribute controller addr",len,1,1);
1292
1293         PASSTHROUGH_IO_OUT(vga,port,src,len);
1294
1295         if (new_addr>VGA_ATTRIBUTE_CONTROLLER_NUM) {
1296             PrintError("vga: ignoring change of attribute controller address to %u (>%u)\n",
1297                        new_addr, VGA_ATTRIBUTE_CONTROLLER_NUM);
1298             //return -1;
1299         } else {
1300             vga->vga_attribute_controller.vga_attribute_controller_addr.val =  *((uint8_t*)src) ;
1301         }
1302         vga->vga_attribute_controller.state=ATTR_DATA;
1303         return len;
1304
1305     } else if (vga->vga_attribute_controller.state==ATTR_DATA) {
1306
1307         uint8_t data = *((uint8_t*)src);
1308         uint8_t index=vga->vga_attribute_controller.vga_attribute_controller_addr.val;  // should mask probably
1309
1310         PrintDebug("vga: attribute controller data write index %d with data=0x%x\n", index,data);
1311
1312         ERR_WRONG_SIZE("write","vga attribute controller data",len,1,1);
1313
1314         PASSTHROUGH_IO_OUT(vga,port,src,len);
1315
1316         vga->vga_attribute_controller.vga_attribute_controller_regs[index] = data;
1317
1318         vga->vga_attribute_controller.state=ATTR_ADDR;
1319
1320         return len;
1321     }
1322
1323     return -1;
1324
1325 }
1326
1327 static int attribute_controller_data_read(struct guest_info *core,
1328                                           uint16_t port,
1329                                           void *dest,
1330                                           uint_t len,
1331                                           void *priv_data)
1332 {
1333     struct vga_internal *vga = (struct vga_internal *) priv_data;
1334     uint8_t index;
1335     uint8_t data;
1336
1337     index=vga->vga_attribute_controller.vga_attribute_controller_addr.val;  // should mask probably
1338     data=vga->vga_attribute_controller.vga_attribute_controller_regs[index];
1339
1340     PrintDebug("vga: attribute controller data read data (index=%d) = 0x%x\n",
1341                index, data);
1342
1343     ERR_WRONG_SIZE("read","vga attribute controller data",len,1,1);
1344
1345     PASSTHROUGH_IO_IN(vga,port,dest,len);
1346
1347     *((uint8_t*)dest) = data;
1348
1349     return len;
1350 }
1351
1352
1353 /*
1354    Note that these guys also have a strange protocol
1355    since they need to squeeze 18 bits of data through
1356    an 8 bit port
1357 */
1358 static int dac_write_address_read(struct guest_info *core,
1359                                   uint16_t port,
1360                                   void *dest,
1361                                   uint_t len,
1362                                   void *priv_data)
1363 {
1364     struct vga_internal *vga = (struct vga_internal *) priv_data;
1365
1366     PrintDebug("vga: dac write address read data=0x%x\n",
1367                vga->vga_dac.vga_dac_write_addr);
1368
1369     ERR_WRONG_SIZE("read","vga dac write addr",len,1,1);
1370
1371     PASSTHROUGH_IO_IN(vga,port,dest,len);
1372
1373     *((uint8_t*)dest) = vga->vga_dac.vga_dac_write_addr;
1374
1375     // This read does not reset the state machine
1376
1377     return len;
1378 }
1379
1380 static int dac_write_address_write(struct guest_info *core,
1381                                    uint16_t port,
1382                                    void *src,
1383                                    uint_t len,
1384                                    void *priv_data)
1385 {
1386     struct vga_internal *vga = (struct vga_internal *) priv_data;
1387     uint8_t new_addr;
1388
1389     new_addr=*((uint8_t*)src);
1390
1391     PrintDebug("vga: dac write address write data=0x%x\n", new_addr);
1392
1393     ERR_WRONG_SIZE("write","vga dac write addr",len,1,1);
1394
1395     PASSTHROUGH_IO_OUT(vga,port,src,len);
1396
1397     // cannot be out of bounds since there are 256 regs
1398
1399     vga->vga_dac.vga_dac_write_addr =  *((uint8_t*)src) ;
1400
1401     // Now we also need to reset the state machine
1402
1403     vga->vga_dac.state=DAC_WRITE;
1404     vga->vga_dac.channel=RED;
1405
1406     return len;
1407 }
1408
1409
1410 static int dac_read_address_read(struct guest_info *core,
1411                                  uint16_t port,
1412                                  void *dest,
1413                                  uint_t len,
1414                                  void *priv_data)
1415 {
1416     struct vga_internal *vga = (struct vga_internal *) priv_data;
1417
1418     PrintDebug("vga: dac read address read data=0x%x\n",
1419                vga->vga_dac.vga_dac_read_addr);
1420
1421     ERR_WRONG_SIZE("read","vga dac read addr",len,1,1);
1422
1423     PASSTHROUGH_IO_IN(vga,port,dest,len);
1424
1425     *((uint8_t*)dest) = vga->vga_dac.vga_dac_read_addr;
1426
1427     // This read does not reset the state machine
1428
1429     return len;
1430 }
1431
1432 static int dac_read_address_write(struct guest_info *core,
1433                                   uint16_t port,
1434                                   void *src,
1435                                   uint_t len,
1436                                   void *priv_data)
1437 {
1438     struct vga_internal *vga = (struct vga_internal *) priv_data;
1439     uint8_t new_addr;
1440
1441     new_addr=*((uint8_t*)src);
1442
1443     PrintDebug("vga: dac read address write data=0x%x\n", new_addr);
1444
1445     ERR_WRONG_SIZE("write","vga dac read addr",len,1,1);
1446
1447     PASSTHROUGH_IO_OUT(vga,port,src,len);
1448
1449     // cannot be out of bounds since there are 256 regs
1450
1451     vga->vga_dac.vga_dac_read_addr =  *((uint8_t*)src) ;
1452
1453     // Now we also need to reset the state machine
1454
1455     vga->vga_dac.state=DAC_READ;
1456     vga->vga_dac.channel=RED;
1457
1458     return len;
1459 }
1460
1461
1462 static int dac_data_read(struct guest_info *core,
1463                          uint16_t port,
1464                          void *dest,
1465                          uint_t len,
1466                          void *priv_data)
1467 {
1468     struct vga_internal *vga = (struct vga_internal *) priv_data;
1469     uint8_t curreg;
1470     uint8_t curchannel;
1471     uint8_t data;
1472
1473     if (vga->vga_dac.state!=DAC_READ) {
1474         PrintError("vga: dac data read while in other state\n");
1475         // results undefined, so we continue
1476     }
1477
1478     ERR_WRONG_SIZE("read","vga dac read data",len,1,1);
1479
1480     PASSTHROUGH_IO_IN(vga,port,dest,len);
1481
1482     curreg = vga->vga_dac.vga_dac_read_addr;
1483     curchannel = vga->vga_dac.channel;
1484     data = (vga->vga_dac.vga_dac_palette[curreg] >> curchannel*8) & 0x3f;
1485
1486     PrintDebug("vga: dac reg %u [%s] = 0x%x\n",
1487                curreg,
1488                curchannel == 0 ? "RED" : curchannel == 1 ? "GREEN"
1489                           : curchannel==2 ? "BLUE" : "BAD CHANNEL",
1490                data);
1491
1492     *((uint8_t*)dest) = data;
1493
1494     curchannel = (curchannel+1)%3;
1495     vga->vga_dac.channel=curchannel;
1496     if (curchannel==0) {
1497         curreg = (curreg + 1) % VGA_DAC_NUM_ENTRIES;
1498     }
1499     vga->vga_dac.vga_dac_read_addr = curreg;
1500     vga->vga_dac.state=DAC_READ;
1501
1502     return len;
1503 }
1504
1505
1506
1507 static int dac_data_write(struct guest_info *core,
1508                           uint16_t port,
1509                           void *src,
1510                           uint_t len,
1511                           void *priv_data)
1512 {
1513     struct vga_internal *vga = (struct vga_internal *) priv_data;
1514     uint8_t curreg;
1515     uint8_t curchannel;
1516     uint8_t data;
1517     vga_palette_reg data32;
1518     vga_palette_reg mask32;
1519
1520     if (vga->vga_dac.state!=DAC_WRITE) {
1521         PrintError("vga: dac data write while in other state\n");
1522         // results undefined, so we continue
1523     }
1524
1525     ERR_WRONG_SIZE("read","vga dac write data",len,1,1);
1526
1527     PASSTHROUGH_IO_OUT(vga,port,src,len);
1528
1529     curreg = vga->vga_dac.vga_dac_write_addr;
1530     curchannel = vga->vga_dac.channel;
1531     data = *((uint8_t *)src);
1532
1533     PrintDebug("vga: dac reg %u [%s] write with 0x%x\n",
1534                curreg,
1535                curchannel == 0 ? "RED" : curchannel == 1 ? "GREEN"
1536                           : curchannel==2 ? "BLUE" : "BAD CHANNEL",
1537                data);
1538
1539     data32 = data & 0x3f ;
1540     data32 <<= curchannel*8;
1541     mask32 = ~(0xff << (curchannel * 8));
1542
1543     vga->vga_dac.vga_dac_palette[curreg] &= mask32;
1544     vga->vga_dac.vga_dac_palette[curreg] |= data32;
1545
1546     curchannel = (curchannel+1)%3;
1547     vga->vga_dac.channel=curchannel;
1548     if (curchannel==0) {
1549         curreg = (curreg + 1) % VGA_DAC_NUM_ENTRIES;
1550     }
1551     vga->vga_dac.vga_dac_write_addr = curreg;
1552     vga->vga_dac.state=DAC_WRITE;
1553
1554     render(vga);
1555
1556     return len;
1557 }
1558
1559
1560
1561 static int dac_pixel_mask_read(struct guest_info *core,
1562                                uint16_t port,
1563                                void *dest,
1564                                uint_t len,
1565                                void *priv_data)
1566 {
1567     struct vga_internal *vga = (struct vga_internal *) priv_data;
1568
1569     PrintDebug("vga: dac pixel mask read data=0x%x\n",
1570                vga->vga_dac.vga_pixel_mask);
1571
1572     ERR_WRONG_SIZE("read","vga pixel mask",len,1,1);
1573
1574     PASSTHROUGH_IO_IN(vga,port,dest,len);
1575
1576     *((uint8_t*)dest) = vga->vga_dac.vga_pixel_mask;
1577
1578     return len;
1579 }
1580
1581 static int dac_pixel_mask_write(struct guest_info *core,
1582                                 uint16_t port,
1583                                 void *src,
1584                                 uint_t len,
1585                                 void *priv_data)
1586 {
1587     struct vga_internal *vga = (struct vga_internal *) priv_data;
1588     uint8_t new_data;
1589
1590     new_data=*((uint8_t*)src);
1591
1592     PrintDebug("vga: dac pixel mask write data=0x%x\n", new_data);
1593
1594     ERR_WRONG_SIZE("write","pixel mask",len,1,1);
1595
1596     PASSTHROUGH_IO_OUT(vga,port,src,len);
1597
1598     vga->vga_dac.vga_pixel_mask =  new_data;
1599
1600     return len;
1601 }
1602
1603 static int init_vga(struct vga_internal *vga)
1604 {
1605     // TODO: startup spec of register contents, if any
1606     PrintError("vga: init_vga is UNIMPLEMTED\n");
1607     return 0;
1608 }
1609
1610 static int free_vga(struct vga_internal *vga)
1611 {
1612     int i;
1613     int ret;
1614     struct vm_device *dev = vga->dev;
1615
1616     // Framebuffer deletion is user's responsibility
1617
1618     //    if (vga->mem_store) {
1619     //  V3_FreePages(v3_hva_to_hpa(vga->mem_store),MEM_REGION_NUM_PAGES);
1620     //  vga->mem_store=0;
1621     //}
1622
1623     for (i=0;i<MAP_NUM;i++) {
1624         if (vga->map[i]) {
1625             V3_FreePages(V3_PAddr(vga->map[i]),MAP_SIZE/4096);
1626             vga->map[i]=0;
1627         }
1628     }
1629
1630     v3_unhook_mem(vga->dev->vm, V3_MEM_CORE_ANY, MEM_REGION_START);
1631
1632     ret = 0;
1633
1634     ret |= v3_dev_unhook_io(dev, VGA_MISC_OUT_READ);
1635     // The following also covers VGA_INPUT_STAT0_READ
1636     ret |= v3_dev_unhook_io(dev, VGA_MISC_OUT_WRITE);
1637     // The following also covers VGA_FEATURE_CTRL_WRITE_MONO
1638     ret |= v3_dev_unhook_io(dev, VGA_INPUT_STAT1_READ_MONO);
1639     // The folowinn also covers VGA FEATURE_CONTROL_WRITE_COLOR
1640     ret |= v3_dev_unhook_io(dev, VGA_INPUT_STAT1_READ_COLOR);
1641     ret |= v3_dev_unhook_io(dev, VGA_FEATURE_CONTROL_READ);
1642
1643     ret |= v3_dev_unhook_io(dev, VGA_VIDEO_SUBSYS_ENABLE);
1644
1645     /* Sequencer registers */
1646     ret |= v3_dev_unhook_io(dev, VGA_SEQUENCER_ADDRESS);
1647     ret |= v3_dev_unhook_io(dev, VGA_SEQUENCER_DATA);
1648
1649     /* CRT controller registers */
1650     ret |= v3_dev_unhook_io(dev, VGA_CRT_CONTROLLER_ADDRESS_MONO);
1651     ret |= v3_dev_unhook_io(dev, VGA_CRT_CONTROLLER_ADDRESS_COLOR);
1652     ret |= v3_dev_unhook_io(dev, VGA_CRT_CONTROLLER_DATA_MONO);
1653     ret |= v3_dev_unhook_io(dev, VGA_CRT_CONTROLLER_DATA_COLOR);
1654
1655     /* graphics controller registers */
1656     ret |= v3_dev_unhook_io(dev, VGA_GRAPHICS_CONTROLLER_ADDRESS);
1657     ret |= v3_dev_unhook_io(dev, VGA_GRAPHICS_CONTROLLER_DATA);
1658
1659     /* attribute controller registers */
1660     ret |= v3_dev_unhook_io(dev, VGA_ATTRIBUTE_CONTROLLER_ADDRESS_AND_WRITE);
1661     ret |= v3_dev_unhook_io(dev, VGA_ATTRIBUTE_CONTROLLER_READ);
1662
1663     /* video DAC palette registers */
1664     ret |= v3_dev_unhook_io(dev, VGA_DAC_WRITE_ADDR);
1665     ret |= v3_dev_unhook_io(dev, VGA_DAC_READ_ADDR);
1666     ret |= v3_dev_unhook_io(dev, VGA_DAC_DATA);
1667     ret |= v3_dev_unhook_io(dev, VGA_DAC_PIXEL_MASK);
1668
1669
1670     V3_Free(vga);
1671
1672     return 0;
1673 }
1674
1675 static struct v3_device_ops dev_ops = {
1676     .free = (int (*)(void *))free_vga,
1677 };
1678
1679
1680 static int vga_init(struct v3_vm_info * vm, v3_cfg_tree_t * cfg) {
1681     struct vga_internal *vga;
1682     int i;
1683     int ret;
1684
1685     char * dev_id = v3_cfg_val(cfg, "ID");
1686     char * passthrough = v3_cfg_val(cfg, "passthrough");
1687
1688     // DETERMINE THE FRAMEBUFFER AND SET IT EARLY
1689     // FRAMEBUFFER IS SUPPLIED BY THE BACKEND
1690
1691     PrintDebug("vga: init_device\n");
1692
1693     vga = (struct vga_internal *)V3_Malloc(sizeof(struct vga_internal));
1694
1695     if (!vga) {
1696         PrintError("vga: cannot allocate\n");
1697         return -1;
1698     }
1699
1700     memset(vga, 0, sizeof(struct vga_internal));
1701
1702     if (passthrough && strcasecmp(passthrough,"enable")==0) {
1703         PrintDebug("vga: enabling passthrough\n");
1704         vga->passthrough=true;
1705     }
1706
1707     // No memory store is allocated since we will use a full memory hook
1708     // The VGA maps can be read as well as written
1709
1710     // Now allocate the maps
1711     for (i=0;i<MAP_NUM;i++) {
1712         vga->map[i] = (vga_map) V3_VAddr((void*)V3_AllocPages(MAP_SIZE/4096));
1713         if (!(vga->map[i])) {
1714             free_vga(vga);
1715         }
1716         memset(vga->map[i],0,MAP_SIZE);
1717     }
1718
1719     struct vm_device * dev = v3_add_device(vm, dev_id, &dev_ops, vga);
1720
1721     if (dev == NULL) {
1722         PrintError("Could not attach device %s\n", dev_id);
1723         free_vga(vga);
1724         return -1;
1725     }
1726
1727     vga->dev = dev;
1728
1729     if (v3_hook_full_mem(vm, V3_MEM_CORE_ANY,
1730                          MEM_REGION_START, MEM_REGION_END,
1731                          &vga_read,
1732                          &vga_write,
1733                          dev) == -1) {
1734         PrintError("vga: memory book failed\n");
1735         v3_remove_device(dev);
1736         return -1;
1737     }
1738
1739     ret = 0;
1740
1741     /* Miscelaneous registers */
1742     ret |= v3_dev_hook_io(dev, VGA_MISC_OUT_READ, &misc_out_read, NULL);
1743     // The following also covers VGA_INPUT_STAT0_READ
1744     ret |= v3_dev_hook_io(dev, VGA_MISC_OUT_WRITE, &input_stat0_read, &misc_out_write);
1745     // The following also covers VGA_FEATURE_CTRL_WRITE_MONO
1746     ret |= v3_dev_hook_io(dev, VGA_INPUT_STAT1_READ_MONO, &input_stat1_read, &feature_control_write);
1747     // The folowinn also covers VGA FEATURE_CONTROL_WRITE_COLOR
1748     ret |= v3_dev_hook_io(dev, VGA_INPUT_STAT1_READ_COLOR, &input_stat1_read, &feature_control_write);
1749     ret |= v3_dev_hook_io(dev, VGA_FEATURE_CONTROL_READ, &feature_control_read, NULL);
1750
1751     ret |= v3_dev_hook_io(dev, VGA_VIDEO_SUBSYS_ENABLE, &video_subsys_enable_read, &video_subsys_enable_write);
1752
1753     /* Sequencer registers */
1754     ret |= v3_dev_hook_io(dev, VGA_SEQUENCER_ADDRESS, &sequencer_address_read, &sequencer_address_write);
1755     ret |= v3_dev_hook_io(dev, VGA_SEQUENCER_DATA, &sequencer_data_read, &sequencer_data_write);
1756
1757     /* CRT controller registers */
1758     ret |= v3_dev_hook_io(dev, VGA_CRT_CONTROLLER_ADDRESS_MONO, &crt_controller_address_read,&crt_controller_address_write);
1759     ret |= v3_dev_hook_io(dev, VGA_CRT_CONTROLLER_ADDRESS_COLOR, &crt_controller_address_read,&crt_controller_address_write);
1760     ret |= v3_dev_hook_io(dev, VGA_CRT_CONTROLLER_DATA_MONO, &crt_controller_data_read,&crt_controller_data_write);
1761     ret |= v3_dev_hook_io(dev, VGA_CRT_CONTROLLER_DATA_COLOR, &crt_controller_data_read,&crt_controller_data_write);
1762
1763     /* graphics controller registers */
1764     ret |= v3_dev_hook_io(dev, VGA_GRAPHICS_CONTROLLER_ADDRESS, &graphics_controller_address_read,&graphics_controller_address_write);
1765     ret |= v3_dev_hook_io(dev, VGA_GRAPHICS_CONTROLLER_DATA, &graphics_controller_data_read,&graphics_controller_data_write);
1766
1767     /* attribute controller registers */
1768     ret |= v3_dev_hook_io(dev, VGA_ATTRIBUTE_CONTROLLER_ADDRESS_AND_WRITE, &attribute_controller_address_read,&attribute_controller_address_and_data_write);
1769     ret |= v3_dev_hook_io(dev, VGA_ATTRIBUTE_CONTROLLER_READ, &attribute_controller_data_read,NULL);
1770
1771     /* video DAC palette registers */
1772     ret |= v3_dev_hook_io(dev, VGA_DAC_WRITE_ADDR, &dac_write_address_read,&dac_write_address_write);
1773     ret |= v3_dev_hook_io(dev, VGA_DAC_READ_ADDR, &dac_read_address_read,&dac_read_address_write);
1774     ret |= v3_dev_hook_io(dev, VGA_DAC_DATA, &dac_data_read, &dac_data_write);
1775     ret |= v3_dev_hook_io(dev, VGA_DAC_PIXEL_MASK, &dac_pixel_mask_read, &dac_pixel_mask_write);
1776
1777     if (ret != 0) {
1778         PrintError("vga: Error allocating VGA I/O ports\n");
1779         v3_remove_device(dev);
1780         return -1;
1781     }
1782
1783     init_vga(vga);
1784
1785     PrintDebug("vga: successfully added and initialized, waiting for framebuffer attach\n");
1786
1787     return 0;
1788
1789 }
1790
1791 device_register("VGA", vga_init);