hw/vga.c

   1 /*
   2  * QEMU VGA Emulator. An S3 86c968 is emulated
   3  *
   4  * Copyright (c) 2003 Fabrice Bellard
   5  *
   6  * Permission is hereby granted, free of charge, to any person obtaining a copy
   7  * of this software and associated documentation files (the "Software"), to deal
   8  * in the Software without restriction, including without limitation the rights
   9  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  10  * copies of the Software, and to permit persons to whom the Software is
  11  * furnished to do so, subject to the following conditions:
  12  *
  13  * The above copyright notice and this permission notice shall be included in
  14  * all copies or substantial portions of the Software.
  15  *
  16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  17  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  18  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  19  * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  20  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  21  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  22  * THE SOFTWARE.
  23  */
  24 #include <stdlib.h>
  25 #include <stdio.h>
  26 #include <stdarg.h>
  27 #include <string.h>
  28 #include <getopt.h>
  29 #include <inttypes.h>
  30 #include <unistd.h>
  31 #include <sys/mman.h>
  32 #include <fcntl.h>
  33 #include <signal.h>
  34 #include <time.h>
  35 #include <sys/time.h>
  36 #include <malloc.h>
  37 #include <termios.h>
  38 #include <sys/poll.h>
  39 #include <errno.h>
  40 #include <sys/wait.h>
  41 #include <netinet/in.h>
  42
  43 #include "cpu.h"
  44 #include "exec-all.h"
  45
  46 #include "vl.h"
  47
  48 #define NO_THUNK_TYPE_SIZE
  49 #include "thunk.h"
  50
  51 //#define DEBUG_VGA
  52 //#define DEBUG_VGA_MEM
  53
  54 #define MSR_COLOR_EMULATION 0x01
  55 #define MSR_PAGE_SELECT     0x20
  56
  57 #define ST01_V_RETRACE      0x08
  58 #define ST01_DISP_ENABLE    0x01
  59
  60 typedef struct VGAState {
  61     uint8_t *vram_ptr;
  62     unsigned long vram_offset;
  63     unsigned int vram_size;
  64     uint32_t latch;
  65     uint8_t sr_index;
  66     uint8_t sr[8];
  67     uint8_t gr_index;
  68     uint8_t gr[16];
  69     uint8_t ar_index;
  70     uint8_t ar[21];
  71     int ar_flip_flop;
  72     uint8_t cr_index;
  73     uint8_t cr[256]; /* CRT registers */
  74     uint8_t msr; /* Misc Output Register */
  75     uint8_t fcr; /* Feature Control Register */
  76     uint8_t st00; /* status 0 */
  77     uint8_t st01; /* status 1 */
  78     uint8_t dac_state;
  79     uint8_t dac_sub_index;
  80     uint8_t dac_read_index;
  81     uint8_t dac_write_index;
  82     uint8_t dac_cache[3]; /* used when writing */
  83     uint8_t palette[768];
  84
  85     /* display refresh support */
  86     DisplayState *ds;
  87     uint32_t font_offsets[2];
  88     int graphic_mode;
  89     uint8_t shift_control;
  90     uint8_t double_scan;
  91     uint32_t line_offset;
  92     uint32_t line_compare;
  93     uint32_t start_addr;
  94     uint8_t last_cw, last_ch;
  95     uint32_t last_width, last_height;
  96     uint8_t cursor_start, cursor_end;
  97     uint32_t cursor_offset;
  98     unsigned int (*rgb_to_pixel)(unsigned int r, unsigned int g, unsigned b);
  99     /* tell for each page if it has been updated since the last time */
 100     uint8_t vram_updated[VGA_RAM_SIZE / 4096];
 101     uint32_t last_palette[256];
 102 #define CH_ATTR_SIZE (160 * 100)
 103     uint32_t last_ch_attr[CH_ATTR_SIZE]; /* XXX: make it dynamic */
 104 } VGAState;
 105
 106 /* force some bits to zero */
 107 static const uint8_t sr_mask[8] = {
 108     (uint8_t)~0xfc,
 109     (uint8_t)~0xc2,
 110     (uint8_t)~0xf0,
 111     (uint8_t)~0xc0,
 112     (uint8_t)~0xf1,
 113     (uint8_t)~0xff,
 114     (uint8_t)~0xff,
 115     (uint8_t)~0x00,
 116 };
 117
 118 static const uint8_t gr_mask[16] = {
 119     (uint8_t)~0xf0, /* 0x00 */
 120     (uint8_t)~0xf0, /* 0x01 */
 121     (uint8_t)~0xf0, /* 0x02 */
 122     (uint8_t)~0xe0, /* 0x03 */
 123     (uint8_t)~0xfc, /* 0x04 */
 124     (uint8_t)~0x84, /* 0x05 */
 125     (uint8_t)~0xf0, /* 0x06 */
 126     (uint8_t)~0xf0, /* 0x07 */
 127     (uint8_t)~0x00, /* 0x08 */
 128     (uint8_t)~0xff, /* 0x09 */
 129     (uint8_t)~0xff, /* 0x0a */
 130     (uint8_t)~0xff, /* 0x0b */
 131     (uint8_t)~0xff, /* 0x0c */
 132     (uint8_t)~0xff, /* 0x0d */
 133     (uint8_t)~0xff, /* 0x0e */
 134     (uint8_t)~0xff, /* 0x0f */
 135 };
 136
 137 #define cbswap_32(__x) \
 138 ((uint32_t)( \
 139                 (((uint32_t)(__x) & (uint32_t)0x000000ffUL) << 24) | \
 140                 (((uint32_t)(__x) & (uint32_t)0x0000ff00UL) <<  8) | \
 141                 (((uint32_t)(__x) & (uint32_t)0x00ff0000UL) >>  8) | \
 142                 (((uint32_t)(__x) & (uint32_t)0xff000000UL) >> 24) ))
 143
 144 #ifdef WORDS_BIGENDIAN
 145 #define PAT(x) cbswap_32(x)
 146 #else
 147 #define PAT(x) (x)
 148 #endif
 149
 150 #ifdef WORDS_BIGENDIAN
 151 #define BIG 1
 152 #else
 153 #define BIG 0
 154 #endif
 155
 156 #ifdef WORDS_BIGENDIAN
 157 #define GET_PLANE(data, p) (((data) >> (24 - (p) * 8)) & 0xff)
 158 #else
 159 #define GET_PLANE(data, p) (((data) >> ((p) * 8)) & 0xff)
 160 #endif
 161
 162 static const uint32_t mask16[16] = {
 163     PAT(0x00000000),
 164     PAT(0x000000ff),
 165     PAT(0x0000ff00),
 166     PAT(0x0000ffff),
 167     PAT(0x00ff0000),
 168     PAT(0x00ff00ff),
 169     PAT(0x00ffff00),
 170     PAT(0x00ffffff),
 171     PAT(0xff000000),
 172     PAT(0xff0000ff),
 173     PAT(0xff00ff00),
 174     PAT(0xff00ffff),
 175     PAT(0xffff0000),
 176     PAT(0xffff00ff),
 177     PAT(0xffffff00),
 178     PAT(0xffffffff),
 179 };
 180
 181 #undef PAT
 182
 183 #ifdef WORDS_BIGENDIAN
 184 #define PAT(x) (x)
 185 #else
 186 #define PAT(x) cbswap_32(x)
 187 #endif
 188
 189 static const uint32_t dmask16[16] = {
 190     PAT(0x00000000),
 191     PAT(0x000000ff),
 192     PAT(0x0000ff00),
 193     PAT(0x0000ffff),
 194     PAT(0x00ff0000),
 195     PAT(0x00ff00ff),
 196     PAT(0x00ffff00),
 197     PAT(0x00ffffff),
 198     PAT(0xff000000),
 199     PAT(0xff0000ff),
 200     PAT(0xff00ff00),
 201     PAT(0xff00ffff),
 202     PAT(0xffff0000),
 203     PAT(0xffff00ff),
 204     PAT(0xffffff00),
 205     PAT(0xffffffff),
 206 };
 207
 208 static const uint32_t dmask4[4] = {
 209     PAT(0x00000000),
 210     PAT(0x0000ffff),
 211     PAT(0xffff0000),
 212     PAT(0xffffffff),
 213 };
 214
 215 static uint32_t expand4[256];
 216 static uint16_t expand2[256];
 217 static uint8_t expand4to8[16];
 218
 219 VGAState vga_state;
 220 int vga_io_memory;
 221
 222 static uint32_t vga_ioport_read(CPUX86State *env, uint32_t addr)
 223 {
 224     VGAState *s = &vga_state;
 225     int val, index;
 226
 227     /* check port range access depending on color/monochrome mode */
 228     if ((addr >= 0x3b0 && addr <= 0x3bf && (s->msr & MSR_COLOR_EMULATION)) ||
 229         (addr >= 0x3d0 && addr <= 0x3df && !(s->msr & MSR_COLOR_EMULATION))) {
 230         val = 0xff;
 231     } else {
 232         switch(addr) {
 233         case 0x3c0:
 234             if (s->ar_flip_flop == 0) {
 235                 val = s->ar_index;
 236             } else {
 237                 val = 0;
 238             }
 239             break;
 240         case 0x3c1:
 241             index = s->ar_index & 0x1f;
 242             if (index < 21)
 243                 val = s->ar[index];
 244             else
 245                 val = 0;
 246             break;
 247         case 0x3c2:
 248             val = s->st00;
 249             break;
 250         case 0x3c4:
 251             val = s->sr_index;
 252             break;
 253         case 0x3c5:
 254             val = s->sr[s->sr_index];
 255             break;
 256         case 0x3c7:
 257             val = s->dac_state;
 258             break;
 259         case 0x3c9:
 260             val = s->palette[s->dac_read_index * 3 + s->dac_sub_index];
 261             if (++s->dac_sub_index == 3) {
 262                 s->dac_sub_index = 0;
 263                 s->dac_read_index++;
 264             }
 265             break;
 266         case 0x3ca:
 267             val = s->fcr;
 268             break;
 269         case 0x3cc:
 270             val = s->msr;
 271             break;
 272         case 0x3ce:
 273             val = s->gr_index;
 274             break;
 275         case 0x3cf:
 276             val = s->gr[s->gr_index];
 277             break;
 278         case 0x3b4:
 279         case 0x3d4:
 280             val = s->cr_index;
 281             break;
 282         case 0x3b5:
 283         case 0x3d5:
 284             val = s->cr[s->cr_index];
 285             break;
 286         case 0x3ba:
 287         case 0x3da:
 288             /* just toggle to fool polling */
 289             s->st01 ^= ST01_V_RETRACE | ST01_DISP_ENABLE;
 290             val = s->st01;
 291             s->ar_flip_flop = 0;
 292             break;
 293         default:
 294             val = 0x00;
 295             break;
 296         }
 297     }
 298 #ifdef DEBUG_VGA
 299     printf("VGA: read addr=0x%04x data=0x%02x\n", addr, val);
 300 #endif
 301     return val;
 302 }
 303
 304 static void vga_ioport_write(CPUX86State *env, uint32_t addr, uint32_t val)
 305 {
 306     VGAState *s = &vga_state;
 307     int index, v;
 308
 309     /* check port range access depending on color/monochrome mode */
 310     if ((addr >= 0x3b0 && addr <= 0x3bf && (s->msr & MSR_COLOR_EMULATION)) ||
 311         (addr >= 0x3d0 && addr <= 0x3df && !(s->msr & MSR_COLOR_EMULATION)))
 312         return;
 313
 314 #ifdef DEBUG_VGA
 315     printf("VGA: write addr=0x%04x data=0x%02x\n", addr, val);
 316 #endif
 317
 318     switch(addr) {
 319     case 0x3c0:
 320         if (s->ar_flip_flop == 0) {
 321             val &= 0x3f;
 322             s->ar_index = val;
 323         } else {
 324             index = s->ar_index & 0x1f;
 325             switch(index) {
 326             case 0x00 ... 0x0f:
 327                 s->ar[index] = val & 0x3f;
 328                 break;
 329             case 0x10:
 330                 s->ar[index] = val & ~0x10;
 331                 break;
 332             case 0x11:
 333                 s->ar[index] = val;
 334                 break;
 335             case 0x12:
 336                 s->ar[index] = val & ~0xc0;
 337                 break;
 338             case 0x13:
 339                 s->ar[index] = val & ~0xf0;
 340                 break;
 341             case 0x14:
 342                 s->ar[index] = val & ~0xf0;
 343                 break;
 344             default:
 345                 break;
 346             }
 347         }
 348         s->ar_flip_flop ^= 1;
 349         break;
 350     case 0x3c2:
 351         s->msr = val & ~0x10;
 352         break;
 353     case 0x3c4:
 354         s->sr_index = val & 7;
 355         break;
 356     case 0x3c5:
 357         s->sr[s->sr_index] = val & sr_mask[s->sr_index];
 358         break;
 359     case 0x3c7:
 360         s->dac_read_index = val;
 361         s->dac_sub_index = 0;
 362         s->dac_state = 3;
 363         break;
 364     case 0x3c8:
 365         s->dac_write_index = val;
 366         s->dac_sub_index = 0;
 367         s->dac_state = 0;
 368         break;
 369     case 0x3c9:
 370         s->dac_cache[s->dac_sub_index] = val;
 371         if (++s->dac_sub_index == 3) {
 372             memcpy(&s->palette[s->dac_write_index * 3], s->dac_cache, 3);
 373             s->dac_sub_index = 0;
 374             s->dac_write_index++;
 375         }
 376         break;
 377     case 0x3ce:
 378         s->gr_index = val & 0x0f;
 379         break;
 380     case 0x3cf:
 381         s->gr[s->gr_index] = val & gr_mask[s->gr_index];
 382         break;
 383     case 0x3b4:
 384     case 0x3d4:
 385         s->cr_index = val;
 386         break;
 387     case 0x3b5:
 388     case 0x3d5:
 389         /* handle CR0-7 protection */
 390         if ((s->cr[11] & 0x80) && s->cr_index <= 7) {
 391             /* can always write bit 4 of CR7 */
 392             if (s->cr_index == 7)
 393                 s->cr[7] = (s->cr[7] & ~0x10) | (val & 0x10);
 394             return;
 395         }
 396         switch(s->cr_index) {
 397         case 0x01: /* horizontal display end */
 398         case 0x07:
 399         case 0x09:
 400         case 0x0c:
 401         case 0x0d:
 402         case 0x12: /* veritcal display end */
 403             s->cr[s->cr_index] = val;
 404             break;
 405
 406             /* S3 registers */
 407         case 0x2d:
 408         case 0x2e:
 409         case 0x2f:
 410         case 0x30:
 411             /* chip ID, cannot write */
 412             break;
 413         case 0x31:
 414             /* update start address */
 415             s->cr[s->cr_index] = val;
 416             v = (val >> 4) & 3;
 417             s->cr[0x69] = (s->cr[69] & ~0x03) | v;
 418             break;
 419         case 0x51:
 420             /* update start address */
 421             s->cr[s->cr_index] = val;
 422             v = val & 3;
 423             s->cr[0x69] = (s->cr[69] & ~0x0c) | (v << 2);
 424             break;
 425         default:
 426             s->cr[s->cr_index] = val;
 427             break;
 428         }
 429         break;
 430     case 0x3ba:
 431     case 0x3da:
 432         s->fcr = val & 0x10;
 433         break;
 434     }
 435 }
 436
 437 /* called for accesses between 0xa0000 and 0xc0000 */
 438 static uint32_t vga_mem_readb(uint32_t addr)
 439 {
 440     VGAState *s = &vga_state;
 441     int memory_map_mode, plane;
 442     uint32_t ret;
 443
 444     /* convert to VGA memory offset */
 445     memory_map_mode = (s->gr[6] >> 2) & 3;
 446     switch(memory_map_mode) {
 447     case 0:
 448         addr -= 0xa0000;
 449         break;
 450     case 1:
 451         addr -= 0xa0000;
 452         if (addr >= 0x10000)
 453             return 0xff;
 454         break;
 455     case 2:
 456         addr -= 0xb0000;
 457         if (addr >= 0x8000)
 458             return 0xff;
 459         break;
 460     default:
 461     case 3:
 462         addr -= 0xb8000;
 463         break;
 464     }
 465
 466     if (s->sr[4] & 0x08) {
 467         /* chain 4 mode : simplest access */
 468         ret = s->vram_ptr[addr];
 469     } else if (s->gr[5] & 0x10) {
 470         /* odd/even mode (aka text mode mapping) */
 471         plane = (s->gr[4] & 2) | (addr & 1);
 472         ret = s->vram_ptr[((addr & ~1) << 1) | plane];
 473     } else {
 474         /* standard VGA latched access */
 475         s->latch = ((uint32_t *)s->vram_ptr)[addr];
 476
 477         if (!(s->gr[5] & 0x08)) {
 478             /* read mode 0 */
 479             plane = s->gr[4];
 480             ret = GET_PLANE(s->latch, plane);
 481         } else {
 482             /* read mode 1 */
 483             ret = (s->latch ^ mask16[s->gr[2]]) & mask16[s->gr[7]];
 484             ret |= ret >> 16;
 485             ret |= ret >> 8;
 486             ret = (~ret) & 0xff;
 487         }
 488     }
 489     return ret;
 490 }
 491
 492 static uint32_t vga_mem_readw(uint32_t addr)
 493 {
 494     uint32_t v;
 495     v = vga_mem_readb(addr);
 496     v |= vga_mem_readb(addr + 1) << 8;
 497     return v;
 498 }
 499
 500 static uint32_t vga_mem_readl(uint32_t addr)
 501 {
 502     uint32_t v;
 503     v = vga_mem_readb(addr);
 504     v |= vga_mem_readb(addr + 1) << 8;
 505     v |= vga_mem_readb(addr + 2) << 16;
 506     v |= vga_mem_readb(addr + 3) << 24;
 507     return v;
 508 }
 509
 510
 511 /* called for accesses between 0xa0000 and 0xc0000 */
 512 void vga_mem_writeb(uint32_t addr, uint32_t val)
 513 {
 514     VGAState *s = &vga_state;
 515     int memory_map_mode, plane, write_mode, b, func_select;
 516     uint32_t write_mask, bit_mask, set_mask;
 517
 518 #ifdef DEBUG_VGA_MEM
 519     printf("vga: [0x%x] = 0x%02x\n", addr, val);
 520 #endif
 521     /* convert to VGA memory offset */
 522     memory_map_mode = (s->gr[6] >> 2) & 3;
 523     switch(memory_map_mode) {
 524     case 0:
 525         addr -= 0xa0000;
 526         break;
 527     case 1:
 528         addr -= 0xa0000;
 529         if (addr >= 0x10000)
 530             return;
 531         break;
 532     case 2:
 533         addr -= 0xb0000;
 534         if (addr >= 0x8000)
 535             return;
 536         break;
 537     default:
 538     case 3:
 539         addr -= 0xb8000;
 540         break;
 541     }
 542
 543     if (s->sr[4] & 0x08) {
 544         /* chain 4 mode : simplest access */
 545         plane = addr & 3;
 546         if (s->sr[2] & (1 << plane)) {
 547             s->vram_ptr[addr] = val;
 548 #ifdef DEBUG_VGA_MEM
 549             printf("vga: chain4: [0x%x]\n", addr);
 550 #endif
 551             s->vram_updated[addr >> 12] = 1;
 552         }
 553     } else if (s->gr[5] & 0x10) {
 554         /* odd/even mode (aka text mode mapping) */
 555         plane = (s->gr[4] & 2) | (addr & 1);
 556         if (s->sr[2] & (1 << plane)) {
 557             addr = ((addr & ~1) << 1) | plane;
 558             s->vram_ptr[addr] = val;
 559 #ifdef DEBUG_VGA_MEM
 560             printf("vga: odd/even: [0x%x]\n", addr);
 561 #endif
 562             s->vram_updated[addr >> 12] = 1;
 563         }
 564     } else {
 565         /* standard VGA latched access */
 566         write_mode = s->gr[5] & 3;
 567         switch(write_mode) {
 568         default:
 569         case 0:
 570             /* rotate */
 571             b = s->gr[3] & 7;
 572             val = ((val >> b) | (val << (8 - b))) & 0xff;
 573             val |= val << 8;
 574             val |= val << 16;
 575
 576             /* apply set/reset mask */
 577             set_mask = mask16[s->gr[1]];
 578             val = (val & ~set_mask) | (mask16[s->gr[0]] & set_mask);
 579             bit_mask = s->gr[8];
 580             break;
 581         case 1:
 582             val = s->latch;
 583             goto do_write;
 584         case 2:
 585             val = mask16[val & 0x0f];
 586             bit_mask = s->gr[8];
 587             break;
 588         case 3:
 589             /* rotate */
 590             b = s->gr[3] & 7;
 591             val = ((val >> b) | (val << (8 - b)));
 592
 593             bit_mask = s->gr[8] & val;
 594             val = mask16[s->gr[0]];
 595             break;
 596         }
 597
 598         /* apply logical operation */
 599         func_select = s->gr[3] >> 3;
 600         switch(func_select) {
 601         case 0:
 602         default:
 603             /* nothing to do */
 604             break;
 605         case 1:
 606             /* and */
 607             val &= s->latch;
 608             break;
 609         case 2:
 610             /* or */
 611             val |= s->latch;
 612             break;
 613         case 3:
 614             /* xor */
 615             val ^= s->latch;
 616             break;
 617         }
 618
 619         /* apply bit mask */
 620         bit_mask |= bit_mask << 8;
 621         bit_mask |= bit_mask << 16;
 622         val = (val & bit_mask) | (s->latch & ~bit_mask);
 623
 624     do_write:
 625         /* mask data according to sr[2] */
 626         write_mask = mask16[s->sr[2]];
 627         ((uint32_t *)s->vram_ptr)[addr] =
 628             (((uint32_t *)s->vram_ptr)[addr] & ~write_mask) |
 629             (val & write_mask);
 630 #ifdef DEBUG_VGA_MEM
 631             printf("vga: latch: [0x%x] mask=0x%08x val=0x%08x\n",
 632                    addr * 4, write_mask, val);
 633 #endif
 634         s->vram_updated[addr >> 10] = 1;
 635     }
 636 }
 637
 638 void vga_mem_writew(uint32_t addr, uint32_t val)
 639 {
 640     vga_mem_writeb(addr, val & 0xff);
 641     vga_mem_writeb(addr + 1, (val >> 8) & 0xff);
 642 }
 643
 644 void vga_mem_writel(uint32_t addr, uint32_t val)
 645 {
 646     vga_mem_writeb(addr, val & 0xff);
 647     vga_mem_writeb(addr + 1, (val >> 8) & 0xff);
 648     vga_mem_writeb(addr + 2, (val >> 16) & 0xff);
 649     vga_mem_writeb(addr + 3, (val >> 24) & 0xff);
 650 }
 651
 652 typedef void vga_draw_glyph8_func(uint8_t *d, int linesize,
 653                              const uint8_t *font_ptr, int h,
 654                              uint32_t fgcol, uint32_t bgcol);
 655 typedef void vga_draw_glyph9_func(uint8_t *d, int linesize,
 656                                   const uint8_t *font_ptr, int h,
 657                                   uint32_t fgcol, uint32_t bgcol, int dup9);
 658 typedef void vga_draw_line_func(VGAState *s1, uint8_t *d,
 659                                 const uint8_t *s, int width);
 660
 661 static inline unsigned int rgb_to_pixel8(unsigned int r, unsigned int g, unsigned b)
 662 {
 663     /* XXX: TODO */
 664     return 0;
 665 }
 666
 667 static inline unsigned int rgb_to_pixel15(unsigned int r, unsigned int g, unsigned b)
 668 {
 669     return ((r >> 3) << 10) | ((g >> 3) << 5) | (b >> 3);
 670 }
 671
 672 static inline unsigned int rgb_to_pixel16(unsigned int r, unsigned int g, unsigned b)
 673 {
 674     return ((r >> 3) << 11) | ((g >> 2) << 5) | (b >> 3);
 675 }
 676
 677 static inline unsigned int rgb_to_pixel32(unsigned int r, unsigned int g, unsigned b)
 678 {
 679     return (r << 16) | (g << 8) | b;
 680 }
 681
 682 #define DEPTH 8
 683 #include "vga_template.h"
 684
 685 #define DEPTH 15
 686 #include "vga_template.h"
 687
 688 #define DEPTH 16
 689 #include "vga_template.h"
 690
 691 #define DEPTH 32
 692 #include "vga_template.h"
 693
 694 static inline int c6_to_8(int v)
 695 {
 696     int b;
 697     v &= 0x3f;
 698     b = v & 1;
 699     return (v << 2) | (b << 1) | b;
 700 }
 701
 702 static unsigned int rgb_to_pixel8_dup(unsigned int r, unsigned int g, unsigned b)
 703 {
 704     unsigned int col;
 705     col = rgb_to_pixel8(r, g, b);
 706     col |= col << 8;
 707     col |= col << 16;
 708     return col;
 709 }
 710
 711 static unsigned int rgb_to_pixel15_dup(unsigned int r, unsigned int g, unsigned b)
 712 {
 713     unsigned int col;
 714     col = rgb_to_pixel15(r, g, b);
 715     col |= col << 16;
 716     return col;
 717 }
 718
 719 static unsigned int rgb_to_pixel16_dup(unsigned int r, unsigned int g, unsigned b)
 720 {
 721     unsigned int col;
 722     col = rgb_to_pixel16(r, g, b);
 723     col |= col << 16;
 724     return col;
 725 }
 726
 727 static unsigned int rgb_to_pixel32_dup(unsigned int r, unsigned int g, unsigned b)
 728 {
 729     unsigned int col;
 730     col = rgb_to_pixel32(r, g, b);
 731     return col;
 732 }
 733
 734 /* return true if the palette was modified */
 735 static int update_palette16(VGAState *s)
 736 {
 737     int full_update, i;
 738     uint32_t v, col, *palette;
 739
 740     full_update = 0;
 741     palette = s->last_palette;
 742     for(i = 0; i < 16; i++) {
 743         v = s->ar[i];
 744         if (s->ar[0x10] & 0x80)
 745             v = ((s->ar[0x14] & 0xf) << 4) | (v & 0xf);
 746         else
 747             v = ((s->ar[0x14] & 0xc) << 4) | (v & 0x3f);
 748         v = v * 3;
 749         col = s->rgb_to_pixel(c6_to_8(s->palette[v]),
 750                               c6_to_8(s->palette[v + 1]),
 751                               c6_to_8(s->palette[v + 2]));
 752         if (col != palette[i]) {
 753             full_update = 1;
 754             palette[i] = col;
 755         }
 756     }
 757     return full_update;
 758 }
 759
 760 /* return true if the palette was modified */
 761 static int update_palette256(VGAState *s)
 762 {
 763     int full_update, i;
 764     uint32_t v, col, *palette;
 765
 766     full_update = 0;
 767     palette = s->last_palette;
 768     v = 0;
 769     for(i = 0; i < 256; i++) {
 770         col = s->rgb_to_pixel(c6_to_8(s->palette[v]),
 771                               c6_to_8(s->palette[v + 1]),
 772                               c6_to_8(s->palette[v + 2]));
 773         if (col != palette[i]) {
 774             full_update = 1;
 775             palette[i] = col;
 776         }
 777         v += 3;
 778     }
 779     return full_update;
 780 }
 781
 782 /* update start_addr and line_offset. Return TRUE if modified */
 783 static int update_basic_params(VGAState *s)
 784 {
 785     int full_update;
 786     uint32_t start_addr, line_offset, line_compare, v;
 787
 788     full_update = 0;
 789     /* compute line_offset in bytes */
 790     v = (s->cr[0x51] >> 4) & 3; /* S3 extension */
 791     if (v == 0)
 792         v = (s->cr[0x43] >> 2) & 1; /* S3 extension */
 793     line_offset = s->cr[0x13] | (v << 8);
 794     line_offset <<= 3;
 795
 796     /* starting address */
 797     start_addr = s->cr[0x0d] | (s->cr[0x0c] << 8);
 798     start_addr |= (s->cr[0x69] & 0x1f) << 16; /* S3 extension */
 799
 800     /* line compare */
 801     line_compare = s->cr[0x18] |
 802         ((s->cr[0x07] & 0x10) << 4) |
 803         ((s->cr[0x09] & 0x40) << 3);
 804
 805     if (line_offset != s->line_offset ||
 806         start_addr != s->start_addr ||
 807         line_compare != s->line_compare) {
 808         s->line_offset = line_offset;
 809         s->start_addr = start_addr;
 810         s->line_compare = line_compare;
 811         full_update = 1;
 812     }
 813     return full_update;
 814 }
 815
 816 static inline int get_depth_index(int depth)
 817 {
 818     switch(depth) {
 819     default:
 820     case 8:
 821         return 0;
 822     case 15:
 823         return 1;
 824     case 16:
 825         return 2;
 826     case 32:
 827         return 3;
 828     }
 829 }
 830
 831 static vga_draw_glyph8_func *vga_draw_glyph8_table[4] = {
 832     vga_draw_glyph8_8,
 833     vga_draw_glyph8_16,
 834     vga_draw_glyph8_16,
 835     vga_draw_glyph8_32,
 836 };
 837
 838 static vga_draw_glyph8_func *vga_draw_glyph16_table[4] = {
 839     vga_draw_glyph16_8,
 840     vga_draw_glyph16_16,
 841     vga_draw_glyph16_16,
 842     vga_draw_glyph16_32,
 843 };
 844
 845 static vga_draw_glyph9_func *vga_draw_glyph9_table[4] = {
 846     vga_draw_glyph9_8,
 847     vga_draw_glyph9_16,
 848     vga_draw_glyph9_16,
 849     vga_draw_glyph9_32,
 850 };
 851
 852 static const uint8_t cursor_glyph[32 * 4] = {
 853     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 854     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 855     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 856     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 857     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 858     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 859     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 860     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 861     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 862     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 863     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 864     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 865     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 866     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 867     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 868     0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
 869 };
 870
 871 /*
 872  * Text mode update
 873  * Missing:
 874  * - double scan
 875  * - double width
 876  * - underline
 877  * - flashing
 878  */
 879 static void vga_draw_text(VGAState *s, int full_update)
 880 {
 881     int cx, cy, cheight, cw, ch, cattr, height, width, ch_attr;
 882     int cx_min, cx_max, linesize, x_incr;
 883     uint32_t offset, fgcol, bgcol, v, cursor_offset;
 884     uint8_t *d1, *d, *src, *s1, *dest, *cursor_ptr;
 885     const uint8_t *font_ptr, *font_base[2];
 886     int dup9, line_offset, depth_index;
 887     uint32_t *palette;
 888     uint32_t *ch_attr_ptr;
 889     vga_draw_glyph8_func *vga_draw_glyph8;
 890     vga_draw_glyph9_func *vga_draw_glyph9;
 891
 892     full_update |= update_palette16(s);
 893     palette = s->last_palette;
 894
 895     /* compute font data address (in plane 2) */
 896     v = s->sr[3];
 897     offset = (((v >> 5) & 1) | ((v >> 1) & 6)) * 8192 * 4 + 2;
 898     if (offset != s->font_offsets[0]) {
 899         s->font_offsets[0] = offset;
 900         full_update = 1;
 901     }
 902     font_base[0] = s->vram_ptr + offset;
 903
 904     offset = (((v >> 4) & 1) | ((v << 1) & 6)) * 8192 * 4 + 2;
 905     font_base[1] = s->vram_ptr + offset;
 906     if (offset != s->font_offsets[1]) {
 907         s->font_offsets[1] = offset;
 908         full_update = 1;
 909     }
 910
 911     full_update |= update_basic_params(s);
 912
 913     line_offset = s->line_offset;
 914     s1 = s->vram_ptr + (s->start_addr * 4);
 915
 916     /* total width & height */
 917     cheight = (s->cr[9] & 0x1f) + 1;
 918     cw = 8;
 919     if (s->sr[1] & 0x01)
 920         cw = 9;
 921     if (s->sr[1] & 0x08)
 922         cw = 16; /* NOTE: no 18 pixel wide */
 923     x_incr = cw * ((s->ds->depth + 7) >> 3);
 924     width = (s->cr[0x01] + 1);
 925     if (s->cr[0x06] == 100) {
 926         /* ugly hack for CGA 160x100x16 - explain me the logic */
 927         height = 100;
 928     } else {
 929         height = s->cr[0x12] |
 930             ((s->cr[0x07] & 0x02) << 7) |
 931             ((s->cr[0x07] & 0x40) << 3);
 932         height = (height + 1) / cheight;
 933     }
 934     if (width != s->last_width || height != s->last_height ||
 935         cw != s->last_cw || cw != s->last_cw) {
 936         dpy_resize(s->ds, width * cw, height * cheight);
 937         s->last_width = width;
 938         s->last_height = height;
 939         s->last_ch = cheight;
 940         s->last_cw = cw;
 941         full_update = 1;
 942     }
 943     cursor_offset = ((s->cr[0x0e] << 8) | s->cr[0x0f]) - s->start_addr;
 944     if (cursor_offset != s->cursor_offset ||
 945         s->cr[0xa] != s->cursor_start ||
 946         s->cr[0xb] != s->cursor_end) {
 947       /* if the cursor position changed, we update the old and new
 948          chars */
 949         if (s->cursor_offset < CH_ATTR_SIZE)
 950             s->last_ch_attr[s->cursor_offset] = -1;
 951         if (cursor_offset < CH_ATTR_SIZE)
 952             s->last_ch_attr[cursor_offset] = -1;
 953         s->cursor_offset = cursor_offset;
 954         s->cursor_start = s->cr[0xa];
 955         s->cursor_end = s->cr[0xb];
 956     }
 957     cursor_ptr = s->vram_ptr + (s->start_addr + cursor_offset) * 4;
 958
 959     depth_index = get_depth_index(s->ds->depth);
 960     if (cw == 16)
 961         vga_draw_glyph8 = vga_draw_glyph16_table[depth_index];
 962     else
 963         vga_draw_glyph8 = vga_draw_glyph8_table[depth_index];
 964     vga_draw_glyph9 = vga_draw_glyph9_table[depth_index];
 965
 966     dest = s->ds->data;
 967     linesize = s->ds->linesize;
 968     ch_attr_ptr = s->last_ch_attr;
 969     for(cy = 0; cy < height; cy++) {
 970         d1 = dest;
 971         src = s1;
 972         cx_min = width;
 973         cx_max = -1;
 974         for(cx = 0; cx < width; cx++) {
 975             ch_attr = *(uint16_t *)src;
 976             if (full_update || ch_attr != *ch_attr_ptr) {
 977                 if (cx < cx_min)
 978                     cx_min = cx;
 979                 if (cx > cx_max)
 980                     cx_max = cx;
 981                 *ch_attr_ptr = ch_attr;
 982 #ifdef WORDS_BIGENDIAN
 983                 ch = ch_attr >> 8;
 984                 cattr = ch_attr & 0xff;
 985 #else
 986                 ch = ch_attr & 0xff;
 987                 cattr = ch_attr >> 8;
 988 #endif
 989                 font_ptr = font_base[(cattr >> 3) & 1];
 990                 font_ptr += 32 * 4 * ch;
 991                 bgcol = palette[cattr >> 4];
 992                 fgcol = palette[cattr & 0x0f];
 993                 if (cw != 9) {
 994                     vga_draw_glyph8(d1, linesize,
 995                                     font_ptr, cheight, fgcol, bgcol);
 996                 } else {
 997                     dup9 = 0;
 998                     if (ch >= 0xb0 && ch <= 0xdf && (s->ar[0x10] & 0x04))
 999                         dup9 = 1;
1000                     vga_draw_glyph9(d1, linesize,
1001                                     font_ptr, cheight, fgcol, bgcol, dup9);
1002                 }
1003                 if (src == cursor_ptr &&
1004                     !(s->cr[0x0a] & 0x20)) {
1005                     int line_start, line_last, h;
1006                     /* draw the cursor */
1007                     line_start = s->cr[0x0a] & 0x1f;
1008                     line_last = s->cr[0x0b] & 0x1f;
1009                     /* XXX: check that */
1010                     if (line_last > cheight - 1)
1011                         line_last = cheight - 1;
1012                     if (line_last >= line_start && line_start < cheight) {
1013                         h = line_last - line_start + 1;
1014                         d = d1 + linesize * line_start;
1015                         if (cw != 9) {
1016                             vga_draw_glyph8(d, linesize,
1017                                             cursor_glyph, h, fgcol, bgcol);
1018                         } else {
1019                             vga_draw_glyph9(d, linesize,
1020                                             cursor_glyph, h, fgcol, bgcol, 1);
1021                         }
1022                     }
1023                 }
1024             }
1025             d1 += x_incr;
1026             src += 4;
1027             ch_attr_ptr++;
1028         }
1029         if (cx_max != -1) {
1030             dpy_update(s->ds, cx_min * cw, cy * cheight,
1031                        (cx_max - cx_min + 1) * cw, cheight);
1032         }
1033         dest += linesize * cheight;
1034         s1 += line_offset;
1035     }
1036 }
1037
1038 enum {
1039     VGA_DRAW_LINE2,
1040     VGA_DRAW_LINE2D2,
1041     VGA_DRAW_LINE4,
1042     VGA_DRAW_LINE4D2,
1043     VGA_DRAW_LINE8D2,
1044     VGA_DRAW_LINE8,
1045     VGA_DRAW_LINE15,
1046     VGA_DRAW_LINE16,
1047     VGA_DRAW_LINE32,
1048     VGA_DRAW_LINE_NB,
1049 };
1050
1051 static vga_draw_line_func *vga_draw_line_table[4 * VGA_DRAW_LINE_NB] = {
1052     vga_draw_line2_8,
1053     vga_draw_line2_16,
1054     vga_draw_line2_16,
1055     vga_draw_line2_32,
1056
1057     vga_draw_line2d2_8,
1058     vga_draw_line2d2_16,
1059     vga_draw_line2d2_16,
1060     vga_draw_line2d2_32,
1061
1062     vga_draw_line4_8,
1063     vga_draw_line4_16,
1064     vga_draw_line4_16,
1065     vga_draw_line4_32,
1066
1067     vga_draw_line4d2_8,
1068     vga_draw_line4d2_16,
1069     vga_draw_line4d2_16,
1070     vga_draw_line4d2_32,
1071
1072     vga_draw_line8d2_8,
1073     vga_draw_line8d2_16,
1074     vga_draw_line8d2_16,
1075     vga_draw_line8d2_32,
1076
1077     vga_draw_line8_8,
1078     vga_draw_line8_16,
1079     vga_draw_line8_16,
1080     vga_draw_line8_32,
1081
1082     vga_draw_line15_8,
1083     vga_draw_line15_15,
1084     vga_draw_line15_16,
1085     vga_draw_line15_32,
1086
1087     vga_draw_line16_8,
1088     vga_draw_line16_15,
1089     vga_draw_line16_16,
1090     vga_draw_line16_32,
1091
1092     vga_draw_line32_8,
1093     vga_draw_line32_15,
1094     vga_draw_line32_16,
1095     vga_draw_line32_32,
1096 };
1097
1098 /*
1099  * graphic modes
1100  * Missing:
1101  * - double scan
1102  * - double width
1103  */
1104 static void vga_draw_graphic(VGAState *s, int full_update)
1105 {
1106     int y1, y, update, page_min, page_max, linesize, y_start, double_scan, mask;
1107     int width, height, shift_control, line_offset, page0, page1, bwidth;
1108     int disp_width, multi_scan, multi_run;
1109     uint8_t *d;
1110     uint32_t v, addr1, addr;
1111     vga_draw_line_func *vga_draw_line;
1112
1113     full_update |= update_basic_params(s);
1114
1115     width = (s->cr[0x01] + 1) * 8;
1116     height = s->cr[0x12] |
1117         ((s->cr[0x07] & 0x02) << 7) |
1118         ((s->cr[0x07] & 0x40) << 3);
1119     height = (height + 1);
1120     disp_width = width;
1121
1122     shift_control = (s->gr[0x05] >> 5) & 3;
1123     double_scan = (s->cr[0x09] & 0x80);
1124     if (shift_control > 1) {
1125         multi_scan = (s->cr[0x09] & 0x1f);
1126     } else {
1127         multi_scan = 0;
1128     }
1129     multi_run = multi_scan;
1130     if (shift_control != s->shift_control ||
1131         double_scan != s->double_scan) {
1132         full_update = 1;
1133         s->shift_control = shift_control;
1134         s->double_scan = double_scan;
1135     }
1136
1137     if (shift_control == 0) {
1138         full_update |= update_palette16(s);
1139         if (s->sr[0x01] & 8) {
1140             v = VGA_DRAW_LINE4D2;
1141             disp_width <<= 1;
1142         } else {
1143             v = VGA_DRAW_LINE4;
1144         }
1145     } else if (shift_control == 1) {
1146         full_update |= update_palette16(s);
1147         if (s->sr[0x01] & 8) {
1148             v = VGA_DRAW_LINE2D2;
1149             disp_width <<= 1;
1150         } else {
1151             v = VGA_DRAW_LINE2;
1152         }
1153     } else {
1154         full_update |= update_palette256(s);
1155         v = VGA_DRAW_LINE8D2;
1156     }
1157     vga_draw_line = vga_draw_line_table[v * 4 + get_depth_index(s->ds->depth)];
1158
1159     if (disp_width != s->last_width ||
1160         height != s->last_height) {
1161         dpy_resize(s->ds, disp_width, height);
1162         s->last_width = disp_width;
1163         s->last_height = height;
1164         full_update = 1;
1165     }
1166
1167     line_offset = s->line_offset;
1168 #if 0
1169     printf("w=%d h=%d v=%d line_offset=%d double_scan=0x%02x cr[0x17]=0x%02x linecmp=%d sr[0x01]=%02x\n",
1170            width, height, v, line_offset, s->cr[9], s->cr[0x17], s->line_compare, s->sr[0x01]);
1171 #endif
1172     addr1 = (s->start_addr * 4);
1173     bwidth = width * 4;
1174     y_start = -1;
1175     page_min = 0x7fffffff;
1176     page_max = -1;
1177     d = s->ds->data;
1178     linesize = s->ds->linesize;
1179     y1 = 0;
1180     for(y = 0; y < height; y++) {
1181         addr = addr1;
1182         if (!(s->cr[0x17] & 1)) {
1183             int shift;
1184             /* CGA compatibility handling */
1185             shift = 14 + ((s->cr[0x17] >> 6) & 1);
1186             addr = (addr & ~(1 << shift)) | ((y1 & 1) << shift);
1187         }
1188         if (!(s->cr[0x17] & 2)) {
1189             addr = (addr & ~0x8000) | ((y1 & 2) << 14);
1190         }
1191         page0 = addr >> 12;
1192         page1 = (addr + bwidth - 1) >> 12;
1193         update = full_update | s->vram_updated[page0] | s->vram_updated[page1];
1194         if ((page1 - page0) > 1) {
1195             /* if wide line, can use another page */
1196             update |= s->vram_updated[page0 + 1];
1197         }
1198         if (update) {
1199             if (y_start < 0)
1200                 y_start = y;
1201             if (page0 < page_min)
1202                 page_min = page0;
1203             if (page1 > page_max)
1204                 page_max = page1;
1205             vga_draw_line(s, d, s->vram_ptr + addr, width);
1206         } else {
1207             if (y_start >= 0) {
1208                 /* flush to display */
1209                 dpy_update(s->ds, 0, y_start,
1210                            disp_width, y - y_start);
1211                 y_start = -1;
1212             }
1213         }
1214         if (!multi_run) {
1215             if (!double_scan || (y & 1) != 0) {
1216                 if (y1 == s->line_compare) {
1217                     addr1 = 0;
1218                 } else {
1219                     mask = (s->cr[0x17] & 3) ^ 3;
1220                     if ((y1 & mask) == mask)
1221                         addr1 += line_offset;
1222                 }
1223                 y1++;
1224             }
1225             multi_run = multi_scan;
1226         } else {
1227             multi_run--;
1228             y1++;
1229         }
1230         d += linesize;
1231     }
1232     if (y_start >= 0) {
1233         /* flush to display */
1234         dpy_update(s->ds, 0, y_start,
1235                    disp_width, y - y_start);
1236     }
1237     /* reset modified pages */
1238     if (page_max != -1) {
1239         memset(s->vram_updated + page_min, 0, page_max - page_min + 1);
1240     }
1241 }
1242
1243 /* draw text terminal (very limited, just for simple boot debug
1244    messages) */
1245 static int last_cursor_pos;
1246
1247 void vga_draw_dumb(VGAState *s)
1248 {
1249     int c, i, cursor_pos, eol;
1250
1251     cursor_pos = s->cr[0x0f] | (s->cr[0x0e] << 8);
1252     eol = 0;
1253     for(i = last_cursor_pos; i < cursor_pos; i++) {
1254         /* XXX: should use vga RAM */
1255         c = phys_ram_base[0xb8000 + (i) * 2];
1256         if (c >= ' ') {
1257             putchar(c);
1258             eol = 0;
1259         } else {
1260             if (!eol)
1261                 putchar('\n');
1262             eol = 1;
1263         }
1264     }
1265     fflush(stdout);
1266     last_cursor_pos = cursor_pos;
1267 }
1268
1269 void vga_update_display(void)
1270 {
1271     VGAState *s = &vga_state;
1272     int full_update, graphic_mode;
1273
1274     if (s->ds->depth == 0) {
1275         vga_draw_dumb(s);
1276     } else {
1277         full_update = 0;
1278         graphic_mode = s->gr[6] & 1;
1279         if (graphic_mode != s->graphic_mode) {
1280             s->graphic_mode = graphic_mode;
1281             full_update = 1;
1282         }
1283         if (graphic_mode)
1284             vga_draw_graphic(s, full_update);
1285         else
1286             vga_draw_text(s, full_update);
1287     }
1288 }
1289
1290 void vga_reset(VGAState *s)
1291 {
1292     memset(s, 0, sizeof(VGAState));
1293     /* chip ID for 8c968 */
1294     s->cr[0x2d] = 0x88;
1295     s->cr[0x2e] = 0xb0;
1296     s->cr[0x2f] = 0x01; /* XXX: check revision code */
1297     s->cr[0x30] = 0xe1;
1298     s->graphic_mode = -1; /* force full update */
1299 }
1300
1301 CPUReadMemoryFunc *vga_mem_read[3] = {
1302     vga_mem_readb,
1303     vga_mem_readw,
1304     vga_mem_readl,
1305 };
1306
1307 CPUWriteMemoryFunc *vga_mem_write[3] = {
1308     vga_mem_writeb,
1309     vga_mem_writew,
1310     vga_mem_writel,
1311 };
1312
1313 int vga_init(DisplayState *ds, uint8_t *vga_ram_base,
1314              unsigned long vga_ram_offset, int vga_ram_size)
1315 {
1316     VGAState *s = &vga_state;
1317     int i, j, v, b;
1318
1319     for(i = 0;i < 256; i++) {
1320         v = 0;
1321         for(j = 0; j < 8; j++) {
1322             v |= ((i >> j) & 1) << (j * 4);
1323         }
1324         expand4[i] = v;
1325
1326         v = 0;
1327         for(j = 0; j < 4; j++) {
1328             v |= ((i >> (2 * j)) & 3) << (j * 4);
1329         }
1330         expand2[i] = v;
1331     }
1332     for(i = 0; i < 16; i++) {
1333         v = 0;
1334         for(j = 0; j < 4; j++) {
1335             b = ((i >> j) & 1);
1336             v |= b << (2 * j);
1337             v |= b << (2 * j + 1);
1338         }
1339         expand4to8[i] = v;
1340     }
1341
1342     vga_reset(s);
1343
1344     switch(ds->depth) {
1345     case 8:
1346         s->rgb_to_pixel = rgb_to_pixel8_dup;
1347         break;
1348     case 15:
1349         s->rgb_to_pixel = rgb_to_pixel15_dup;
1350         break;
1351     default:
1352     case 16:
1353         s->rgb_to_pixel = rgb_to_pixel16_dup;
1354         break;
1355     case 32:
1356         s->rgb_to_pixel = rgb_to_pixel32_dup;
1357         break;
1358     }
1359
1360     s->vram_ptr = vga_ram_base;
1361     s->vram_offset = vga_ram_offset;
1362     s->vram_size = vga_ram_size;
1363     s->ds = ds;
1364
1365     register_ioport_write(0x3c0, 16, vga_ioport_write, 1);
1366
1367     register_ioport_write(0x3b4, 2, vga_ioport_write, 1);
1368     register_ioport_write(0x3d4, 2, vga_ioport_write, 1);
1369     register_ioport_write(0x3ba, 1, vga_ioport_write, 1);
1370     register_ioport_write(0x3da, 1, vga_ioport_write, 1);
1371
1372     register_ioport_read(0x3c0, 16, vga_ioport_read, 1);
1373
1374     register_ioport_read(0x3b4, 2, vga_ioport_read, 1);
1375     register_ioport_read(0x3d4, 2, vga_ioport_read, 1);
1376     register_ioport_read(0x3ba, 1, vga_ioport_read, 1);
1377     register_ioport_read(0x3da, 1, vga_ioport_read, 1);
1378
1379     vga_io_memory = cpu_register_io_memory(0, vga_mem_read, vga_mem_write);
1380     cpu_register_physical_memory(0xa0000, 0x20000, vga_io_memory);
1381     return 0;
1382 }