vgasrc/vga.c

   1 // VGA bios implementation
   2 //
   3 // Copyright (C) 2009  Kevin O'Connor <kevin@koconnor.net>
   4 // Copyright (C) 2001-2008 the LGPL VGABios developers Team
   5 //
   6 // This file may be distributed under the terms of the GNU LGPLv3 license.
   7
   8
   9 // TODO:
  10 //  * introduce "struct vregs", or add ebp to struct bregs.
  11 //  * Integrate vga_modes/pallete?/line_to_vpti/dac_regs/video_param_table
  12 //  * define structs for save/restore state
  13 //  * review correctness of converted asm by comparing with RBIL
  14 //  * eliminate unimplemented() calls
  15 //  * eliminate DEBUG defs
  16 //  * more syntax cleanups
  17 //  * refactor redundant code into sub-functions
  18 //  * See if there is a method to the in/out stuff that can be encapsulated.
  19 //  * remove "biosfn" prefixes
  20 //  * don't hardcode 0xc000
  21 //  * add defs for 0xa000/0xb800
  22 //  * verify all funcs static
  23 //
  24 //  * separate code into separate files
  25 //  * extract hw code from bios interfaces
  26 //
  27 //  * convert vbe/clext code
  28
  29 #include "bregs.h" // struct bregs
  30 #include "biosvar.h" // GET_BDA
  31 #include "util.h" // memset
  32 #include "vgatables.h" // vga_modes
  33
  34 // XXX
  35 #define CONFIG_VBE 0
  36 #define CONFIG_CIRRUS 0
  37
  38 // XXX
  39 #define DEBUG_VGA_POST 1
  40 #define DEBUG_VGA_10 3
  41
  42 #define SET_VGA(var, val) SET_FARVAR(0xc000, (var), (val))
  43
  44
  45 // ===================================================================
  46 //
  47 // Video Utils
  48 //
  49 // ===================================================================
  50
  51 // -------------------------------------------------------------------
  52 static u8
  53 find_vga_entry(u8 mode)
  54 {
  55     u8 i;
  56     for (i = 0; i <= MODE_MAX; i++)
  57         if (GET_GLOBAL(vga_modes[i].svgamode) == mode)
  58             return i;
  59     return 0xFF;
  60 }
  61
  62 inline void
  63 call16_vgaint(u32 eax, u32 ebx)
  64 {
  65     asm volatile(
  66         "int $0x10\n"
  67         "cli\n"
  68         "cld"
  69         :
  70         : "a"(eax), "b"(ebx)
  71         : "cc", "memory");
  72 }
  73
  74 // XXX
  75 inline void
  76 memcpy16_far(u16 d_seg, void *d_far, u16 s_seg, const void *s_far, size_t len)
  77 {
  78     memcpy_far(d_seg, d_far, s_seg, s_far, len);
  79 }
  80
  81
  82 // ===================================================================
  83 //
  84 // BIOS functions
  85 //
  86 // ===================================================================
  87
  88 // -------------------------------------------------------------------
  89 static void
  90 biosfn_perform_gray_scale_summing(u16 start, u16 count)
  91 {
  92     u8 r, g, b;
  93     u16 i;
  94     u16 index;
  95
  96     inb(VGAREG_ACTL_RESET);
  97     outb(0x00, VGAREG_ACTL_ADDRESS);
  98
  99     for (index = 0; index < count; index++) {
 100         // set read address and switch to read mode
 101         outb(start, VGAREG_DAC_READ_ADDRESS);
 102         // get 6-bit wide RGB data values
 103         r = inb(VGAREG_DAC_DATA);
 104         g = inb(VGAREG_DAC_DATA);
 105         b = inb(VGAREG_DAC_DATA);
 106
 107         // intensity = ( 0.3 * Red ) + ( 0.59 * Green ) + ( 0.11 * Blue )
 108         i = ((77 * r + 151 * g + 28 * b) + 0x80) >> 8;
 109
 110         if (i > 0x3f)
 111             i = 0x3f;
 112
 113         // set write address and switch to write mode
 114         outb(start, VGAREG_DAC_WRITE_ADDRESS);
 115         // write new intensity value
 116         outb(i & 0xff, VGAREG_DAC_DATA);
 117         outb(i & 0xff, VGAREG_DAC_DATA);
 118         outb(i & 0xff, VGAREG_DAC_DATA);
 119         start++;
 120     }
 121     inb(VGAREG_ACTL_RESET);
 122     outb(0x20, VGAREG_ACTL_ADDRESS);
 123 }
 124
 125 // -------------------------------------------------------------------
 126 static void
 127 biosfn_set_cursor_shape(u8 CH, u8 CL)
 128 {
 129     u16 cheight, curs, crtc_addr;
 130     u8 modeset_ctl;
 131
 132     CH &= 0x3f;
 133     CL &= 0x1f;
 134
 135     curs = (CH << 8) + CL;
 136     SET_BDA(cursor_type, curs);
 137
 138     modeset_ctl = GET_BDA(modeset_ctl);
 139     cheight = GET_BDA(char_height);
 140     if ((modeset_ctl & 0x01) && (cheight > 8) && (CL < 8) && (CH < 0x20)) {
 141         if (CL != (CH + 1)) {
 142             CH = ((CH + 1) * cheight / 8) - 1;
 143         } else {
 144             CH = ((CL + 1) * cheight / 8) - 2;
 145         }
 146         CL = ((CL + 1) * cheight / 8) - 1;
 147     }
 148     // CTRC regs 0x0a and 0x0b
 149     crtc_addr = GET_BDA(crtc_address);
 150     outb(0x0a, crtc_addr);
 151     outb(CH, crtc_addr + 1);
 152     outb(0x0b, crtc_addr);
 153     outb(CL, crtc_addr + 1);
 154 }
 155
 156 // -------------------------------------------------------------------
 157 static void
 158 biosfn_set_cursor_pos(u8 page, u16 cursor)
 159 {
 160     u8 xcurs, ycurs, current;
 161     u16 nbcols, nbrows, address, crtc_addr;
 162
 163     // Should not happen...
 164     if (page > 7)
 165         return;
 166
 167     // Bios cursor pos
 168     SET_BDA(cursor_pos[page], cursor);
 169
 170     // Set the hardware cursor
 171     current = GET_BDA(video_page);
 172     if (page == current) {
 173         // Get the dimensions
 174         nbcols = GET_BDA(video_cols);
 175         nbrows = GET_BDA(video_rows) + 1;
 176
 177         xcurs = cursor & 0x00ff;
 178         ycurs = (cursor & 0xff00) >> 8;
 179
 180         // Calculate the address knowing nbcols nbrows and page num
 181         address =
 182             SCREEN_IO_START(nbcols, nbrows, page) + xcurs + ycurs * nbcols;
 183
 184         // CRTC regs 0x0e and 0x0f
 185         crtc_addr = GET_BDA(crtc_address);
 186         outb(0x0e, crtc_addr);
 187         outb((address & 0xff00) >> 8, crtc_addr + 1);
 188         outb(0x0f, crtc_addr);
 189         outb(address & 0x00ff, crtc_addr + 1);
 190     }
 191 }
 192
 193 // -------------------------------------------------------------------
 194 static void
 195 biosfn_get_cursor_pos(u8 page, u16 *shape, u16 *pos)
 196 {
 197     // Default
 198     *shape = 0;
 199     *pos = 0;
 200     if (page > 7)
 201         return;
 202
 203     // FIXME should handle VGA 14/16 lines
 204     *shape = GET_BDA(cursor_type);
 205     *pos = GET_BDA(cursor_pos[page]);
 206 }
 207
 208 // -------------------------------------------------------------------
 209 static void
 210 biosfn_set_active_page(u8 page)
 211 {
 212     u16 cursor, dummy, crtc_addr;
 213     u16 nbcols, nbrows, address;
 214     u8 mode, line;
 215
 216     if (page > 7)
 217         return;
 218
 219     // Get the mode
 220     mode = GET_BDA(video_mode);
 221     line = find_vga_entry(mode);
 222     if (line == 0xFF)
 223         return;
 224
 225     // Get pos curs pos for the right page
 226     biosfn_get_cursor_pos(page, &dummy, &cursor);
 227
 228     if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
 229         // Get the dimensions
 230         nbcols = GET_BDA(video_cols);
 231         nbrows = GET_BDA(video_rows) + 1;
 232
 233         // Calculate the address knowing nbcols nbrows and page num
 234         address = SCREEN_MEM_START(nbcols, nbrows, page);
 235         SET_BDA(video_pagestart, address);
 236
 237         // Start address
 238         address = SCREEN_IO_START(nbcols, nbrows, page);
 239     } else {
 240         address = page * GET_GLOBAL(video_param_table[GET_GLOBAL(line_to_vpti[line])].slength);
 241     }
 242
 243     // CRTC regs 0x0c and 0x0d
 244     crtc_addr = GET_BDA(crtc_address);
 245     outb(0x0c, crtc_addr);
 246     outb((address & 0xff00) >> 8, crtc_addr + 1);
 247     outb(0x0d, crtc_addr);
 248     outb(address & 0x00ff, crtc_addr + 1);
 249
 250     // And change the BIOS page
 251     SET_BDA(video_page, page);
 252
 253     dprintf(1, "Set active page %02x address %04x\n", page, address);
 254
 255     // Display the cursor, now the page is active
 256     biosfn_set_cursor_pos(page, cursor);
 257 }
 258
 259 static void
 260 biosfn_set_video_mode(u8 mode)
 261 {                               // mode: Bit 7 is 1 if no clear screen
 262     // Should we clear the screen ?
 263     u8 noclearmem = mode & 0x80;
 264     u8 line, mmask, *palette, vpti;
 265     u16 i, twidth, theightm1, cheight;
 266     u8 modeset_ctl, video_ctl, vga_switches;
 267
 268     if (CONFIG_CIRRUS)
 269         cirrus_set_video_mode(mode);
 270
 271 #ifdef VBE
 272     if (vbe_has_vbe_display())
 273         dispi_set_enable(VBE_DISPI_DISABLED);
 274 #endif
 275
 276     // The real mode
 277     mode = mode & 0x7f;
 278
 279     // find the entry in the video modes
 280     line = find_vga_entry(mode);
 281
 282     dprintf(1, "mode search %02x found line %02x\n", mode, line);
 283
 284     if (line == 0xFF)
 285         return;
 286
 287     vpti = GET_GLOBAL(line_to_vpti[line]);
 288     twidth = GET_GLOBAL(video_param_table[vpti].twidth);
 289     theightm1 = GET_GLOBAL(video_param_table[vpti].theightm1);
 290     cheight = GET_GLOBAL(video_param_table[vpti].cheight);
 291
 292     // Read the bios vga control
 293     video_ctl = GET_BDA(video_ctl);
 294
 295     // Read the bios vga switches
 296     vga_switches = GET_BDA(video_switches);
 297
 298     // Read the bios mode set control
 299     modeset_ctl = GET_BDA(modeset_ctl);
 300
 301     // Then we know the number of lines
 302 // FIXME
 303
 304     // if palette loading (bit 3 of modeset ctl = 0)
 305     if ((modeset_ctl & 0x08) == 0) {    // Set the PEL mask
 306         outb(GET_GLOBAL(vga_modes[line].pelmask), VGAREG_PEL_MASK);
 307
 308         // Set the whole dac always, from 0
 309         outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
 310
 311         // From which palette
 312         switch (GET_GLOBAL(vga_modes[line].dacmodel)) {
 313         default:
 314         case 0:
 315             palette = palette0;
 316             break;
 317         case 1:
 318             palette = palette1;
 319             break;
 320         case 2:
 321             palette = palette2;
 322             break;
 323         case 3:
 324             palette = palette3;
 325             break;
 326         }
 327         // Always 256*3 values
 328         for (i = 0; i < 0x0100; i++) {
 329             if (i <= GET_GLOBAL(dac_regs[GET_GLOBAL(vga_modes[line].dacmodel)])) {
 330                 outb(GET_GLOBAL(palette[(i * 3) + 0]), VGAREG_DAC_DATA);
 331                 outb(GET_GLOBAL(palette[(i * 3) + 1]), VGAREG_DAC_DATA);
 332                 outb(GET_GLOBAL(palette[(i * 3) + 2]), VGAREG_DAC_DATA);
 333             } else {
 334                 outb(0, VGAREG_DAC_DATA);
 335                 outb(0, VGAREG_DAC_DATA);
 336                 outb(0, VGAREG_DAC_DATA);
 337             }
 338         }
 339         if ((modeset_ctl & 0x02) == 0x02)
 340             biosfn_perform_gray_scale_summing(0x00, 0x100);
 341     }
 342     // Reset Attribute Ctl flip-flop
 343     inb(VGAREG_ACTL_RESET);
 344
 345     // Set Attribute Ctl
 346     for (i = 0; i <= 0x13; i++) {
 347         outb(i, VGAREG_ACTL_ADDRESS);
 348         outb(GET_GLOBAL(video_param_table[vpti].actl_regs[i])
 349              , VGAREG_ACTL_WRITE_DATA);
 350     }
 351     outb(0x14, VGAREG_ACTL_ADDRESS);
 352     outb(0x00, VGAREG_ACTL_WRITE_DATA);
 353
 354     // Set Sequencer Ctl
 355     outb(0, VGAREG_SEQU_ADDRESS);
 356     outb(0x03, VGAREG_SEQU_DATA);
 357     for (i = 1; i <= 4; i++) {
 358         outb(i, VGAREG_SEQU_ADDRESS);
 359         outb(GET_GLOBAL(video_param_table[vpti].sequ_regs[i - 1])
 360              , VGAREG_SEQU_DATA);
 361     }
 362
 363     // Set Grafx Ctl
 364     for (i = 0; i <= 8; i++) {
 365         outb(i, VGAREG_GRDC_ADDRESS);
 366         outb(GET_GLOBAL(video_param_table[vpti].grdc_regs[i])
 367              , VGAREG_GRDC_DATA);
 368     }
 369
 370     // Set CRTC address VGA or MDA
 371     u16 crtc_addr = VGAREG_VGA_CRTC_ADDRESS;
 372     if (GET_GLOBAL(vga_modes[line].memmodel) == MTEXT)
 373         crtc_addr = VGAREG_MDA_CRTC_ADDRESS;
 374
 375     // Disable CRTC write protection
 376     outw(0x0011, crtc_addr);
 377     // Set CRTC regs
 378     for (i = 0; i <= 0x18; i++) {
 379         outb(i, crtc_addr);
 380         outb(GET_GLOBAL(video_param_table[vpti].crtc_regs[i]), crtc_addr + 1);
 381     }
 382
 383     // Set the misc register
 384     outb(GET_GLOBAL(video_param_table[vpti].miscreg), VGAREG_WRITE_MISC_OUTPUT);
 385
 386     // Enable video
 387     outb(0x20, VGAREG_ACTL_ADDRESS);
 388     inb(VGAREG_ACTL_RESET);
 389
 390     if (noclearmem == 0x00) {
 391         if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
 392             memset16_far(GET_GLOBAL(vga_modes[line].sstart)
 393                          , 0, 0x0720, 0x4000); // 32k
 394         } else {
 395             if (mode < 0x0d) {
 396                 memset16_far(GET_GLOBAL(vga_modes[line].sstart)
 397                              , 0, 0x0000, 0x4000);     // 32k
 398             } else {
 399                 outb(0x02, VGAREG_SEQU_ADDRESS);
 400                 mmask = inb(VGAREG_SEQU_DATA);
 401                 outb(0x0f, VGAREG_SEQU_DATA);   // all planes
 402                 memset16_far(GET_GLOBAL(vga_modes[line].sstart)
 403                              , 0, 0x0000, 0x8000);     // 64k
 404                 outb(mmask, VGAREG_SEQU_DATA);
 405             }
 406         }
 407     }
 408     // Set the BIOS mem
 409     SET_BDA(video_mode, mode);
 410     SET_BDA(video_cols, twidth);
 411     SET_BDA(video_pagesize, GET_GLOBAL(video_param_table[vpti].slength));
 412     SET_BDA(crtc_address, crtc_addr);
 413     SET_BDA(video_rows, theightm1);
 414     SET_BDA(char_height, cheight);
 415     SET_BDA(video_ctl, (0x60 | noclearmem));
 416     SET_BDA(video_switches, 0xF9);
 417     SET_BDA(modeset_ctl, GET_BDA(modeset_ctl) & 0x7f);
 418
 419     // FIXME We nearly have the good tables. to be reworked
 420     SET_BDA(dcc_index, 0x08);   // 8 is VGA should be ok for now
 421     SET_BDA(video_savetable_ptr, (u32)video_save_pointer_table);
 422     SET_BDA(video_savetable_seg, 0xc000);
 423
 424     // FIXME
 425     SET_BDA(video_msr, 0x00); // Unavailable on vanilla vga, but...
 426     SET_BDA(video_pal, 0x00); // Unavailable on vanilla vga, but...
 427
 428     // Set cursor shape
 429     if (GET_GLOBAL(vga_modes[line].class) == TEXT)
 430         biosfn_set_cursor_shape(0x06, 0x07);
 431     // Set cursor pos for page 0..7
 432     for (i = 0; i < 8; i++)
 433         biosfn_set_cursor_pos(i, 0x0000);
 434
 435     // Set active page 0
 436     biosfn_set_active_page(0x00);
 437
 438     // Write the fonts in memory
 439     if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
 440         call16_vgaint(0x1104, 0);
 441         call16_vgaint(0x1103, 0);
 442     }
 443     // Set the ints 0x1F and 0x43
 444     SET_IVT(0x1f, 0xC000, (u32)&vgafont8[128 * 8]);
 445
 446     switch (cheight) {
 447     case 8:
 448         SET_IVT(0x43, 0xC000, (u32)vgafont8);
 449         break;
 450     case 14:
 451         SET_IVT(0x43, 0xC000, (u32)vgafont14);
 452         break;
 453     case 16:
 454         SET_IVT(0x43, 0xC000, (u32)vgafont16);
 455         break;
 456     }
 457 }
 458
 459 // -------------------------------------------------------------------
 460 static void
 461 vgamem_copy_pl4(u8 xstart, u8 ysrc, u8 ydest, u8 cols, u8 nbcols,
 462                 u8 cheight)
 463 {
 464     u16 src = ysrc * cheight * nbcols + xstart;
 465     u16 dest = ydest * cheight * nbcols + xstart;
 466     outw(0x0105, VGAREG_GRDC_ADDRESS);
 467     u8 i;
 468     for (i = 0; i < cheight; i++)
 469         memcpy_far(0xa000, (void*)(dest + i * nbcols)
 470                    , 0xa000, (void*)(src + i * nbcols), cols);
 471     outw(0x0005, VGAREG_GRDC_ADDRESS);
 472 }
 473
 474 // -------------------------------------------------------------------
 475 static void
 476 vgamem_fill_pl4(u8 xstart, u8 ystart, u8 cols, u8 nbcols, u8 cheight,
 477                 u8 attr)
 478 {
 479     u16 dest = ystart * cheight * nbcols + xstart;
 480     outw(0x0205, VGAREG_GRDC_ADDRESS);
 481     u8 i;
 482     for (i = 0; i < cheight; i++)
 483         memset_far(0xa000, (void*)(dest + i * nbcols), attr, cols);
 484     outw(0x0005, VGAREG_GRDC_ADDRESS);
 485 }
 486
 487 // -------------------------------------------------------------------
 488 static void
 489 vgamem_copy_cga(u8 xstart, u8 ysrc, u8 ydest, u8 cols, u8 nbcols,
 490                 u8 cheight)
 491 {
 492     u16 src = ((ysrc * cheight * nbcols) >> 1) + xstart;
 493     u16 dest = ((ydest * cheight * nbcols) >> 1) + xstart;
 494     u8 i;
 495     for (i = 0; i < cheight; i++)
 496         if (i & 1)
 497             memcpy_far(0xb800, (void*)(0x2000 + dest + (i >> 1) * nbcols)
 498                        , 0xb800, (void*)(0x2000 + src + (i >> 1) * nbcols)
 499                        , cols);
 500         else
 501             memcpy_far(0xb800, (void*)(dest + (i >> 1) * nbcols)
 502                        , 0xb800, (void*)(src + (i >> 1) * nbcols), cols);
 503 }
 504
 505 // -------------------------------------------------------------------
 506 static void
 507 vgamem_fill_cga(u8 xstart, u8 ystart, u8 cols, u8 nbcols, u8 cheight,
 508                 u8 attr)
 509 {
 510     u16 dest = ((ystart * cheight * nbcols) >> 1) + xstart;
 511     u8 i;
 512     for (i = 0; i < cheight; i++)
 513         if (i & 1)
 514             memset_far(0xb800, (void*)(0x2000 + dest + (i >> 1) * nbcols)
 515                        , attr, cols);
 516         else
 517             memset_far(0xb800, (void*)(dest + (i >> 1) * nbcols), attr, cols);
 518 }
 519
 520 // -------------------------------------------------------------------
 521 static void
 522 biosfn_scroll(u8 nblines, u8 attr, u8 rul, u8 cul, u8 rlr, u8 clr, u8 page,
 523               u8 dir)
 524 {
 525     // page == 0xFF if current
 526
 527     u8 mode, line, cheight, bpp, cols;
 528     u16 nbcols, nbrows, i;
 529
 530     if (rul > rlr)
 531         return;
 532     if (cul > clr)
 533         return;
 534
 535     // Get the mode
 536     mode = GET_BDA(video_mode);
 537     line = find_vga_entry(mode);
 538     if (line == 0xFF)
 539         return;
 540
 541     // Get the dimensions
 542     nbrows = GET_BDA(video_rows) + 1;
 543     nbcols = GET_BDA(video_cols);
 544
 545     // Get the current page
 546     if (page == 0xFF)
 547         page = GET_BDA(video_page);
 548
 549     if (rlr >= nbrows)
 550         rlr = nbrows - 1;
 551     if (clr >= nbcols)
 552         clr = nbcols - 1;
 553     if (nblines > nbrows)
 554         nblines = 0;
 555     cols = clr - cul + 1;
 556
 557     if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
 558         // Compute the address
 559         void *address = (void*)(SCREEN_MEM_START(nbcols, nbrows, page));
 560         dprintf(1, "Scroll, address %p (%d %d %02x)\n"
 561                 , address, nbrows, nbcols, page);
 562
 563         if (nblines == 0 && rul == 0 && cul == 0 && rlr == nbrows - 1
 564             && clr == nbcols - 1) {
 565             memset16_far(GET_GLOBAL(vga_modes[line].sstart), address
 566                          , (u16)attr * 0x100 + ' ', nbrows * nbcols);
 567         } else {                // if Scroll up
 568             if (dir == SCROLL_UP) {
 569                 for (i = rul; i <= rlr; i++) {
 570                     if ((i + nblines > rlr) || (nblines == 0))
 571                         memset16_far(GET_GLOBAL(vga_modes[line].sstart)
 572                                      , address + (i * nbcols + cul) * 2
 573                                      , (u16)attr * 0x100 + ' ', cols);
 574                     else
 575                         memcpy16_far(GET_GLOBAL(vga_modes[line].sstart)
 576                                      , address + (i * nbcols + cul) * 2
 577                                      , GET_GLOBAL(vga_modes[line].sstart)
 578                                      , (void*)(((i + nblines) * nbcols + cul) * 2)
 579                                      , cols);
 580                 }
 581             } else {
 582                 for (i = rlr; i >= rul; i--) {
 583                     if ((i < rul + nblines) || (nblines == 0))
 584                         memset16_far(GET_GLOBAL(vga_modes[line].sstart)
 585                                      , address + (i * nbcols + cul) * 2
 586                                      , (u16)attr * 0x100 + ' ', cols);
 587                     else
 588                         memcpy16_far(GET_GLOBAL(vga_modes[line].sstart)
 589                                      , address + (i * nbcols + cul) * 2
 590                                      , GET_GLOBAL(vga_modes[line].sstart)
 591                                      , (void*)(((i - nblines) * nbcols + cul) * 2)
 592                                      , cols);
 593                     if (i > rlr)
 594                         break;
 595                 }
 596             }
 597         }
 598     } else {
 599         // FIXME gfx mode not complete
 600         cheight = GET_GLOBAL(video_param_table[GET_GLOBAL(line_to_vpti[line])].cheight);
 601         switch (GET_GLOBAL(vga_modes[line].memmodel)) {
 602         case PLANAR4:
 603         case PLANAR1:
 604             if (nblines == 0 && rul == 0 && cul == 0 && rlr == nbrows - 1
 605                 && clr == nbcols - 1) {
 606                 outw(0x0205, VGAREG_GRDC_ADDRESS);
 607                 memset_far(GET_GLOBAL(vga_modes[line].sstart), 0, attr,
 608                            nbrows * nbcols * cheight);
 609                 outw(0x0005, VGAREG_GRDC_ADDRESS);
 610             } else {            // if Scroll up
 611                 if (dir == SCROLL_UP) {
 612                     for (i = rul; i <= rlr; i++) {
 613                         if ((i + nblines > rlr) || (nblines == 0))
 614                             vgamem_fill_pl4(cul, i, cols, nbcols, cheight,
 615                                             attr);
 616                         else
 617                             vgamem_copy_pl4(cul, i + nblines, i, cols,
 618                                             nbcols, cheight);
 619                     }
 620                 } else {
 621                     for (i = rlr; i >= rul; i--) {
 622                         if ((i < rul + nblines) || (nblines == 0))
 623                             vgamem_fill_pl4(cul, i, cols, nbcols, cheight,
 624                                             attr);
 625                         else
 626                             vgamem_copy_pl4(cul, i, i - nblines, cols,
 627                                             nbcols, cheight);
 628                         if (i > rlr)
 629                             break;
 630                     }
 631                 }
 632             }
 633             break;
 634         case CGA:
 635             bpp = GET_GLOBAL(vga_modes[line].pixbits);
 636             if (nblines == 0 && rul == 0 && cul == 0 && rlr == nbrows - 1
 637                 && clr == nbcols - 1) {
 638                 memset_far(GET_GLOBAL(vga_modes[line].sstart), 0, attr,
 639                            nbrows * nbcols * cheight * bpp);
 640             } else {
 641                 if (bpp == 2) {
 642                     cul <<= 1;
 643                     cols <<= 1;
 644                     nbcols <<= 1;
 645                 }
 646                 // if Scroll up
 647                 if (dir == SCROLL_UP) {
 648                     for (i = rul; i <= rlr; i++) {
 649                         if ((i + nblines > rlr) || (nblines == 0))
 650                             vgamem_fill_cga(cul, i, cols, nbcols, cheight,
 651                                             attr);
 652                         else
 653                             vgamem_copy_cga(cul, i + nblines, i, cols,
 654                                             nbcols, cheight);
 655                     }
 656                 } else {
 657                     for (i = rlr; i >= rul; i--) {
 658                         if ((i < rul + nblines) || (nblines == 0))
 659                             vgamem_fill_cga(cul, i, cols, nbcols, cheight,
 660                                             attr);
 661                         else
 662                             vgamem_copy_cga(cul, i, i - nblines, cols,
 663                                             nbcols, cheight);
 664                         if (i > rlr)
 665                             break;
 666                     }
 667                 }
 668             }
 669             break;
 670         default:
 671             dprintf(1, "Scroll in graphics mode\n");
 672 #ifdef DEBUG
 673             unimplemented();
 674 #endif
 675         }
 676     }
 677 }
 678
 679 // -------------------------------------------------------------------
 680 static void
 681 biosfn_read_char_attr(u8 page, u16 *car)
 682 {
 683     u8 xcurs, ycurs, mode, line;
 684     u16 nbcols, nbrows;
 685     u16 cursor, dummy;
 686
 687     // Get the mode
 688     mode = GET_BDA(video_mode);
 689     line = find_vga_entry(mode);
 690     if (line == 0xFF)
 691         return;
 692
 693     // Get the cursor pos for the page
 694     biosfn_get_cursor_pos(page, &dummy, &cursor);
 695     xcurs = cursor & 0x00ff;
 696     ycurs = (cursor & 0xff00) >> 8;
 697
 698     // Get the dimensions
 699     nbrows = GET_BDA(video_rows) + 1;
 700     nbcols = GET_BDA(video_cols);
 701
 702     if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
 703         // Compute the address
 704         u16 *address = (void*)(SCREEN_MEM_START(nbcols, nbrows, page)
 705                                + (xcurs + ycurs * nbcols) * 2);
 706
 707         *car = GET_FARVAR(GET_GLOBAL(vga_modes[line].sstart), *address);
 708     } else {
 709         // FIXME gfx mode
 710 #ifdef DEBUG
 711         unimplemented();
 712 #endif
 713     }
 714 }
 715
 716 // -------------------------------------------------------------------
 717 static void
 718 write_gfx_char_pl4(u8 car, u8 attr, u8 xcurs, u8 ycurs, u8 nbcols,
 719                    u8 cheight)
 720 {
 721     u8 i, j, mask;
 722     u8 *fdata;
 723     u16 addr, src;
 724
 725     switch (cheight) {
 726     case 14:
 727         fdata = vgafont14;
 728         break;
 729     case 16:
 730         fdata = vgafont16;
 731         break;
 732     default:
 733         fdata = vgafont8;
 734     }
 735     addr = xcurs + ycurs * cheight * nbcols;
 736     src = car * cheight;
 737     outw(0x0f02, VGAREG_SEQU_ADDRESS);
 738     outw(0x0205, VGAREG_GRDC_ADDRESS);
 739     if (attr & 0x80)
 740         outw(0x1803, VGAREG_GRDC_ADDRESS);
 741     else
 742         outw(0x0003, VGAREG_GRDC_ADDRESS);
 743     for (i = 0; i < cheight; i++) {
 744         u8 *dest = (void*)(addr + i * nbcols);
 745         for (j = 0; j < 8; j++) {
 746             mask = 0x80 >> j;
 747             outw((mask << 8) | 0x08, VGAREG_GRDC_ADDRESS);
 748             GET_FARVAR(0xa000, *dest);
 749             if (GET_GLOBAL(fdata[src + i]) & mask)
 750                 SET_FARVAR(0xa000, *dest, attr & 0x0f);
 751             else
 752                 SET_FARVAR(0xa000, *dest, 0x00);
 753         }
 754     }
 755     outw(0xff08, VGAREG_GRDC_ADDRESS);
 756     outw(0x0005, VGAREG_GRDC_ADDRESS);
 757     outw(0x0003, VGAREG_GRDC_ADDRESS);
 758 }
 759
 760 // -------------------------------------------------------------------
 761 static void
 762 write_gfx_char_cga(u8 car, u8 attr, u8 xcurs, u8 ycurs, u8 nbcols, u8 bpp)
 763 {
 764     u8 *fdata = vgafont8;
 765     u16 addr = (xcurs * bpp) + ycurs * 320;
 766     u16 src = car * 8;
 767     u8 i;
 768     for (i = 0; i < 8; i++) {
 769         u8 *dest = (void*)(addr + (i >> 1) * 80);
 770         if (i & 1)
 771             dest += 0x2000;
 772         u8 mask = 0x80;
 773         if (bpp == 1) {
 774             u8 data = 0;
 775             if (attr & 0x80)
 776                 data = GET_FARVAR(0xb800, *dest);
 777             u8 j;
 778             for (j = 0; j < 8; j++) {
 779                 if (GET_GLOBAL(fdata[src + i]) & mask) {
 780                     if (attr & 0x80)
 781                         data ^= (attr & 0x01) << (7 - j);
 782                     else
 783                         data |= (attr & 0x01) << (7 - j);
 784                 }
 785                 mask >>= 1;
 786             }
 787             SET_FARVAR(0xb800, *dest, data);
 788         } else {
 789             while (mask > 0) {
 790                 u8 data = 0;
 791                 if (attr & 0x80)
 792                     data = GET_FARVAR(0xb800, *dest);
 793                 u8 j;
 794                 for (j = 0; j < 4; j++) {
 795                     if (GET_GLOBAL(fdata[src + i]) & mask) {
 796                         if (attr & 0x80)
 797                             data ^= (attr & 0x03) << ((3 - j) * 2);
 798                         else
 799                             data |= (attr & 0x03) << ((3 - j) * 2);
 800                     }
 801                     mask >>= 1;
 802                 }
 803                 SET_FARVAR(0xb800, *dest, data);
 804                 dest += 1;
 805             }
 806         }
 807     }
 808 }
 809
 810 // -------------------------------------------------------------------
 811 static void
 812 write_gfx_char_lin(u8 car, u8 attr, u8 xcurs, u8 ycurs, u8 nbcols)
 813 {
 814     u8 *fdata = vgafont8;
 815     u16 addr = xcurs * 8 + ycurs * nbcols * 64;
 816     u16 src = car * 8;
 817     u8 i;
 818     for (i = 0; i < 8; i++) {
 819         u8 *dest = (void*)(addr + i * nbcols * 8);
 820         u8 mask = 0x80;
 821         u8 j;
 822         for (j = 0; j < 8; j++) {
 823             u8 data = 0x00;
 824             if (GET_GLOBAL(fdata[src + i]) & mask)
 825                 data = attr;
 826             SET_FARVAR(0xa000, dest[j], data);
 827             mask >>= 1;
 828         }
 829     }
 830 }
 831
 832 // -------------------------------------------------------------------
 833 static void
 834 biosfn_write_char_attr(u8 car, u8 page, u8 attr, u16 count)
 835 {
 836     u8 cheight, xcurs, ycurs, mode, line, bpp;
 837     u16 nbcols, nbrows;
 838     u16 cursor, dummy;
 839
 840     // Get the mode
 841     mode = GET_BDA(video_mode);
 842     line = find_vga_entry(mode);
 843     if (line == 0xFF)
 844         return;
 845
 846     // Get the cursor pos for the page
 847     biosfn_get_cursor_pos(page, &dummy, &cursor);
 848     xcurs = cursor & 0x00ff;
 849     ycurs = (cursor & 0xff00) >> 8;
 850
 851     // Get the dimensions
 852     nbrows = GET_BDA(video_rows) + 1;
 853     nbcols = GET_BDA(video_cols);
 854
 855     if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
 856         // Compute the address
 857         void *address = (void*)(SCREEN_MEM_START(nbcols, nbrows, page)
 858                                 + (xcurs + ycurs * nbcols) * 2);
 859
 860         dummy = ((u16)attr << 8) + car;
 861         memset16_far(GET_GLOBAL(vga_modes[line].sstart), address, dummy, count);
 862     } else {
 863         // FIXME gfx mode not complete
 864         cheight = GET_GLOBAL(video_param_table[GET_GLOBAL(line_to_vpti[line])].cheight);
 865         bpp = GET_GLOBAL(vga_modes[line].pixbits);
 866         while ((count-- > 0) && (xcurs < nbcols)) {
 867             switch (GET_GLOBAL(vga_modes[line].memmodel)) {
 868             case PLANAR4:
 869             case PLANAR1:
 870                 write_gfx_char_pl4(car, attr, xcurs, ycurs, nbcols,
 871                                    cheight);
 872                 break;
 873             case CGA:
 874                 write_gfx_char_cga(car, attr, xcurs, ycurs, nbcols, bpp);
 875                 break;
 876             case LINEAR8:
 877                 write_gfx_char_lin(car, attr, xcurs, ycurs, nbcols);
 878                 break;
 879 #ifdef DEBUG
 880             default:
 881                 unimplemented();
 882 #endif
 883             }
 884             xcurs++;
 885         }
 886     }
 887 }
 888
 889 // -------------------------------------------------------------------
 890 static void
 891 biosfn_write_char_only(u8 car, u8 page, u8 attr, u16 count)
 892 {
 893     u8 cheight, xcurs, ycurs, mode, line, bpp;
 894     u16 nbcols, nbrows;
 895     u16 cursor, dummy;
 896
 897     // Get the mode
 898     mode = GET_BDA(video_mode);
 899     line = find_vga_entry(mode);
 900     if (line == 0xFF)
 901         return;
 902
 903     // Get the cursor pos for the page
 904     biosfn_get_cursor_pos(page, &dummy, &cursor);
 905     xcurs = cursor & 0x00ff;
 906     ycurs = (cursor & 0xff00) >> 8;
 907
 908     // Get the dimensions
 909     nbrows = GET_BDA(video_rows) + 1;
 910     nbcols = GET_BDA(video_cols);
 911
 912     if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
 913         // Compute the address
 914         u8 *address = (void*)(SCREEN_MEM_START(nbcols, nbrows, page)
 915                               + (xcurs + ycurs * nbcols) * 2);
 916         while (count-- > 0) {
 917             SET_FARVAR(GET_GLOBAL(vga_modes[line].sstart), *address, car);
 918             address += 2;
 919         }
 920     } else {
 921         // FIXME gfx mode not complete
 922         cheight = GET_GLOBAL(video_param_table[GET_GLOBAL(line_to_vpti[line])].cheight);
 923         bpp = GET_GLOBAL(vga_modes[line].pixbits);
 924         while ((count-- > 0) && (xcurs < nbcols)) {
 925             switch (GET_GLOBAL(vga_modes[line].memmodel)) {
 926             case PLANAR4:
 927             case PLANAR1:
 928                 write_gfx_char_pl4(car, attr, xcurs, ycurs, nbcols,
 929                                    cheight);
 930                 break;
 931             case CGA:
 932                 write_gfx_char_cga(car, attr, xcurs, ycurs, nbcols, bpp);
 933                 break;
 934             case LINEAR8:
 935                 write_gfx_char_lin(car, attr, xcurs, ycurs, nbcols);
 936                 break;
 937 #ifdef DEBUG
 938             default:
 939                 unimplemented();
 940 #endif
 941             }
 942             xcurs++;
 943         }
 944     }
 945 }
 946
 947
 948
 949 // -------------------------------------------------------------------
 950 static void
 951 biosfn_set_border_color(struct bregs *regs)
 952 {
 953     inb(VGAREG_ACTL_RESET);
 954     outb(0x00, VGAREG_ACTL_ADDRESS);
 955     u8 al = regs->bl & 0x0f;
 956     if (al & 0x08)
 957         al += 0x08;
 958     outb(al, VGAREG_ACTL_WRITE_DATA);
 959     u8 bl = regs->bl & 0x10;
 960
 961     int i;
 962     for (i = 1; i < 4; i++) {
 963         outb(i, VGAREG_ACTL_ADDRESS);
 964
 965         al = inb(VGAREG_ACTL_READ_DATA);
 966         al &= 0xef;
 967         al |= bl;
 968         outb(al, VGAREG_ACTL_WRITE_DATA);
 969     }
 970     outb(0x20, VGAREG_ACTL_ADDRESS);
 971 }
 972
 973 static void
 974 biosfn_set_palette(struct bregs *regs)
 975 {
 976     inb(VGAREG_ACTL_RESET);
 977     u8 bl = regs->bl & 0x01;
 978     int i;
 979     for (i = 1; i < 4; i++) {
 980         outb(i, VGAREG_ACTL_ADDRESS);
 981
 982         u8 al = inb(VGAREG_ACTL_READ_DATA);
 983         al &= 0xfe;
 984         al |= bl;
 985         outb(al, VGAREG_ACTL_WRITE_DATA);
 986     }
 987     outb(0x20, VGAREG_ACTL_ADDRESS);
 988 }
 989
 990 // -------------------------------------------------------------------
 991 static void
 992 biosfn_write_pixel(u8 BH, u8 AL, u16 CX, u16 DX)
 993 {
 994     u8 mask, attr, data;
 995
 996     // Get the mode
 997     u8 mode = GET_BDA(video_mode);
 998     u8 line = find_vga_entry(mode);
 999     if (line == 0xFF)
1000         return;
1001     if (GET_GLOBAL(vga_modes[line].class) == TEXT)
1002         return;
1003
1004     u8 *addr;
1005     switch (GET_GLOBAL(vga_modes[line].memmodel)) {
1006     case PLANAR4:
1007     case PLANAR1:
1008         addr = (void*)(CX / 8 + DX * GET_BDA(video_cols));
1009         mask = 0x80 >> (CX & 0x07);
1010         outw((mask << 8) | 0x08, VGAREG_GRDC_ADDRESS);
1011         outw(0x0205, VGAREG_GRDC_ADDRESS);
1012         data = GET_FARVAR(0xa000, *addr);
1013         if (AL & 0x80)
1014             outw(0x1803, VGAREG_GRDC_ADDRESS);
1015         SET_FARVAR(0xa000, *addr, AL);
1016         outw(0xff08, VGAREG_GRDC_ADDRESS);
1017         outw(0x0005, VGAREG_GRDC_ADDRESS);
1018         outw(0x0003, VGAREG_GRDC_ADDRESS);
1019         break;
1020     case CGA:
1021         if (GET_GLOBAL(vga_modes[line].pixbits) == 2)
1022             addr = (void*)((CX >> 2) + (DX >> 1) * 80);
1023         else
1024             addr = (void*)((CX >> 3) + (DX >> 1) * 80);
1025         if (DX & 1)
1026             addr += 0x2000;
1027         data = GET_FARVAR(0xb800, *addr);
1028         if (GET_GLOBAL(vga_modes[line].pixbits) == 2) {
1029             attr = (AL & 0x03) << ((3 - (CX & 0x03)) * 2);
1030             mask = 0x03 << ((3 - (CX & 0x03)) * 2);
1031         } else {
1032             attr = (AL & 0x01) << (7 - (CX & 0x07));
1033             mask = 0x01 << (7 - (CX & 0x07));
1034         }
1035         if (AL & 0x80) {
1036             data ^= attr;
1037         } else {
1038             data &= ~mask;
1039             data |= attr;
1040         }
1041         SET_FARVAR(0xb800, *addr, data);
1042         break;
1043     case LINEAR8:
1044         addr = (void*)(CX + DX * (GET_BDA(video_cols) * 8));
1045         SET_FARVAR(0xa000, *addr, AL);
1046         break;
1047 #ifdef DEBUG
1048     default:
1049         unimplemented();
1050 #endif
1051     }
1052 }
1053
1054 // -------------------------------------------------------------------
1055 static void
1056 biosfn_read_pixel(u8 BH, u16 CX, u16 DX, u16 *AX)
1057 {
1058     u8 mode, line, mask, attr, data, i;
1059
1060     // Get the mode
1061     mode = GET_BDA(video_mode);
1062     line = find_vga_entry(mode);
1063     if (line == 0xFF)
1064         return;
1065     if (GET_GLOBAL(vga_modes[line].class) == TEXT)
1066         return;
1067
1068     u8 *addr;
1069     switch (GET_GLOBAL(vga_modes[line].memmodel)) {
1070     case PLANAR4:
1071     case PLANAR1:
1072         addr = (void*)(CX / 8 + DX * GET_BDA(video_cols));
1073         mask = 0x80 >> (CX & 0x07);
1074         attr = 0x00;
1075         for (i = 0; i < 4; i++) {
1076             outw((i << 8) | 0x04, VGAREG_GRDC_ADDRESS);
1077             data = GET_FARVAR(0xa000, *addr) & mask;
1078             if (data > 0)
1079                 attr |= (0x01 << i);
1080         }
1081         break;
1082     case CGA:
1083         addr = (void*)((CX >> 2) + (DX >> 1) * 80);
1084         if (DX & 1)
1085             addr += 0x2000;
1086         data = GET_FARVAR(0xb800, *addr);
1087         if (GET_GLOBAL(vga_modes[line].pixbits) == 2)
1088             attr = (data >> ((3 - (CX & 0x03)) * 2)) & 0x03;
1089         else
1090             attr = (data >> (7 - (CX & 0x07))) & 0x01;
1091         break;
1092     case LINEAR8:
1093         addr = (void*)(CX + DX * (GET_BDA(video_cols) * 8));
1094         attr = GET_FARVAR(0xa000, *addr);
1095         break;
1096     default:
1097 #ifdef DEBUG
1098         unimplemented();
1099 #endif
1100         attr = 0;
1101     }
1102     *AX = (*AX & 0xff00) | attr;
1103 }
1104
1105 // -------------------------------------------------------------------
1106 static void
1107 biosfn_write_teletype(u8 car, u8 page, u8 attr, u8 flag)
1108 {                               // flag = WITH_ATTR / NO_ATTR
1109     u8 cheight, xcurs, ycurs, mode, line, bpp;
1110     u16 nbcols, nbrows;
1111     u16 cursor, dummy;
1112
1113     // special case if page is 0xff, use current page
1114     if (page == 0xff)
1115         page = GET_BDA(video_page);
1116
1117     // Get the mode
1118     mode = GET_BDA(video_mode);
1119     line = find_vga_entry(mode);
1120     if (line == 0xFF)
1121         return;
1122
1123     // Get the cursor pos for the page
1124     biosfn_get_cursor_pos(page, &dummy, &cursor);
1125     xcurs = cursor & 0x00ff;
1126     ycurs = (cursor & 0xff00) >> 8;
1127
1128     // Get the dimensions
1129     nbrows = GET_BDA(video_rows) + 1;
1130     nbcols = GET_BDA(video_cols);
1131
1132     switch (car) {
1133     case 7:
1134         //FIXME should beep
1135         break;
1136
1137     case 8:
1138         if (xcurs > 0)
1139             xcurs--;
1140         break;
1141
1142     case '\r':
1143         xcurs = 0;
1144         break;
1145
1146     case '\n':
1147         ycurs++;
1148         break;
1149
1150     case '\t':
1151         do {
1152             biosfn_write_teletype(' ', page, attr, flag);
1153             biosfn_get_cursor_pos(page, &dummy, &cursor);
1154             xcurs = cursor & 0x00ff;
1155             ycurs = (cursor & 0xff00) >> 8;
1156         } while (xcurs % 8 == 0);
1157         break;
1158
1159     default:
1160
1161         if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
1162             // Compute the address
1163             u8 *address = (void*)(SCREEN_MEM_START(nbcols, nbrows, page)
1164                                   + (xcurs + ycurs * nbcols) * 2);
1165             // Write the char
1166             SET_FARVAR(GET_GLOBAL(vga_modes[line].sstart), address[0], car);
1167             if (flag == WITH_ATTR)
1168                 SET_FARVAR(GET_GLOBAL(vga_modes[line].sstart), address[1], attr);
1169         } else {
1170             // FIXME gfx mode not complete
1171             cheight = GET_GLOBAL(video_param_table[GET_GLOBAL(line_to_vpti[line])].cheight);
1172             bpp = GET_GLOBAL(vga_modes[line].pixbits);
1173             switch (GET_GLOBAL(vga_modes[line].memmodel)) {
1174             case PLANAR4:
1175             case PLANAR1:
1176                 write_gfx_char_pl4(car, attr, xcurs, ycurs, nbcols,
1177                                    cheight);
1178                 break;
1179             case CGA:
1180                 write_gfx_char_cga(car, attr, xcurs, ycurs, nbcols, bpp);
1181                 break;
1182             case LINEAR8:
1183                 write_gfx_char_lin(car, attr, xcurs, ycurs, nbcols);
1184                 break;
1185 #ifdef DEBUG
1186             default:
1187                 unimplemented();
1188 #endif
1189             }
1190         }
1191         xcurs++;
1192     }
1193
1194     // Do we need to wrap ?
1195     if (xcurs == nbcols) {
1196         xcurs = 0;
1197         ycurs++;
1198     }
1199     // Do we need to scroll ?
1200     if (ycurs == nbrows) {
1201         if (GET_GLOBAL(vga_modes[line].class) == TEXT)
1202             biosfn_scroll(0x01, 0x07, 0, 0, nbrows - 1, nbcols - 1, page,
1203                           SCROLL_UP);
1204         else
1205             biosfn_scroll(0x01, 0x00, 0, 0, nbrows - 1, nbcols - 1, page,
1206                           SCROLL_UP);
1207         ycurs -= 1;
1208     }
1209     // Set the cursor for the page
1210     cursor = ycurs;
1211     cursor <<= 8;
1212     cursor += xcurs;
1213     biosfn_set_cursor_pos(page, cursor);
1214 }
1215
1216 // -------------------------------------------------------------------
1217 static void
1218 biosfn_get_video_mode(struct bregs *regs)
1219 {
1220     regs->bh = GET_BDA(video_page);
1221     regs->al = GET_BDA(video_mode) | (GET_BDA(video_ctl) & 0x80);
1222     regs->ah = GET_BDA(video_cols);
1223 }
1224
1225 // -------------------------------------------------------------------
1226 static void
1227 biosfn_set_overscan_border_color(struct bregs *regs)
1228 {
1229     inb(VGAREG_ACTL_RESET);
1230     outb(0x11, VGAREG_ACTL_ADDRESS);
1231     outb(regs->bh, VGAREG_ACTL_WRITE_DATA);
1232     outb(0x20, VGAREG_ACTL_ADDRESS);
1233 }
1234
1235 // -------------------------------------------------------------------
1236 static void
1237 biosfn_set_all_palette_reg(struct bregs *regs)
1238 {
1239     inb(VGAREG_ACTL_RESET);
1240
1241     u8 *data = (u8*)(regs->dx + 0);
1242     int i;
1243     for (i = 0; i < 0x10; i++) {
1244         outb(i, VGAREG_ACTL_ADDRESS);
1245         u8 val = GET_FARVAR(regs->es, *data);
1246         outb(val, VGAREG_ACTL_WRITE_DATA);
1247         data++;
1248     }
1249     outb(0x11, VGAREG_ACTL_ADDRESS);
1250     outb(GET_FARVAR(regs->es, *data), VGAREG_ACTL_WRITE_DATA);
1251     outb(0x20, VGAREG_ACTL_ADDRESS);
1252 }
1253
1254 // -------------------------------------------------------------------
1255 static void
1256 biosfn_toggle_intensity(struct bregs *regs)
1257 {
1258     inb(VGAREG_ACTL_RESET);
1259     outb(0x10, VGAREG_ACTL_ADDRESS);
1260     u8 val = (inb(VGAREG_ACTL_READ_DATA) & 0x7f) | ((regs->bl & 0x01) << 3);
1261     outb(val, VGAREG_ACTL_WRITE_DATA);
1262     outb(0x20, VGAREG_ACTL_ADDRESS);
1263 }
1264
1265 // -------------------------------------------------------------------
1266 void
1267 biosfn_set_single_palette_reg(u8 reg, u8 val)
1268 {
1269     inb(VGAREG_ACTL_RESET);
1270     outb(reg, VGAREG_ACTL_ADDRESS);
1271     outb(val, VGAREG_ACTL_WRITE_DATA);
1272     outb(0x20, VGAREG_ACTL_ADDRESS);
1273 }
1274
1275 // -------------------------------------------------------------------
1276 u8
1277 biosfn_get_single_palette_reg(u8 reg)
1278 {
1279     inb(VGAREG_ACTL_RESET);
1280     outb(reg, VGAREG_ACTL_ADDRESS);
1281     u8 v = inb(VGAREG_ACTL_READ_DATA);
1282     inb(VGAREG_ACTL_RESET);
1283     outb(0x20, VGAREG_ACTL_ADDRESS);
1284     return v;
1285 }
1286
1287 // -------------------------------------------------------------------
1288 static void
1289 biosfn_read_overscan_border_color(struct bregs *regs)
1290 {
1291     inb(VGAREG_ACTL_RESET);
1292     outb(0x11, VGAREG_ACTL_ADDRESS);
1293     regs->bh = inb(VGAREG_ACTL_READ_DATA);
1294     inb(VGAREG_ACTL_RESET);
1295     outb(0x20, VGAREG_ACTL_ADDRESS);
1296 }
1297
1298 // -------------------------------------------------------------------
1299 static void
1300 biosfn_get_all_palette_reg(struct bregs *regs)
1301 {
1302     u8 *data = (u8*)(regs->dx + 0);
1303     int i;
1304     for (i = 0; i < 0x10; i++) {
1305         inb(VGAREG_ACTL_RESET);
1306         outb(i, VGAREG_ACTL_ADDRESS);
1307         SET_FARVAR(regs->es, *data, inb(VGAREG_ACTL_READ_DATA));
1308         data++;
1309     }
1310     inb(VGAREG_ACTL_RESET);
1311     outb(0x11, VGAREG_ACTL_ADDRESS);
1312     SET_FARVAR(regs->es, *data, inb(VGAREG_ACTL_READ_DATA));
1313     inb(VGAREG_ACTL_RESET);
1314     outb(0x20, VGAREG_ACTL_ADDRESS);
1315 }
1316
1317 // -------------------------------------------------------------------
1318 static void
1319 biosfn_set_single_dac_reg(struct bregs *regs)
1320 {
1321     outb(regs->bl, VGAREG_DAC_WRITE_ADDRESS);
1322     outb(regs->dh, VGAREG_DAC_DATA);
1323     outb(regs->ch, VGAREG_DAC_DATA);
1324     outb(regs->cl, VGAREG_DAC_DATA);
1325 }
1326
1327 // -------------------------------------------------------------------
1328 static void
1329 biosfn_set_all_dac_reg(struct bregs *regs)
1330 {
1331     outb(regs->bl, VGAREG_DAC_WRITE_ADDRESS);
1332     u8 *data = (u8*)(regs->dx + 0);
1333     int count = regs->cx;
1334     while (count) {
1335         outb(GET_FARVAR(regs->es, *data), VGAREG_DAC_DATA);
1336         data++;
1337         outb(GET_FARVAR(regs->es, *data), VGAREG_DAC_DATA);
1338         data++;
1339         outb(GET_FARVAR(regs->es, *data), VGAREG_DAC_DATA);
1340         data++;
1341         count--;
1342     }
1343 }
1344
1345 // -------------------------------------------------------------------
1346 static void
1347 biosfn_select_video_dac_color_page(struct bregs *regs)
1348 {
1349     inb(VGAREG_ACTL_RESET);
1350     outb(0x10, VGAREG_ACTL_ADDRESS);
1351     u8 val = inb(VGAREG_ACTL_READ_DATA);
1352     if (!(regs->bl & 0x01)) {
1353         val = (val & 0x7f) | (regs->bh << 7);
1354         outb(val, VGAREG_ACTL_WRITE_DATA);
1355         outb(0x20, VGAREG_ACTL_ADDRESS);
1356         return;
1357     }
1358     inb(VGAREG_ACTL_RESET);
1359     outb(0x14, VGAREG_ACTL_ADDRESS);
1360     u8 bh = regs->bh;
1361     if (!(val & 0x80))
1362         bh <<= 2;
1363     bh &= 0x0f;
1364     outb(bh, VGAREG_ACTL_WRITE_DATA);
1365     outb(0x20, VGAREG_ACTL_ADDRESS);
1366 }
1367
1368 // -------------------------------------------------------------------
1369 static void
1370 biosfn_read_single_dac_reg(struct bregs *regs)
1371 {
1372     outb(regs->bl, VGAREG_DAC_READ_ADDRESS);
1373     regs->dh = inb(VGAREG_DAC_DATA);
1374     regs->ch = inb(VGAREG_DAC_DATA);
1375     regs->cl = inb(VGAREG_DAC_DATA);
1376 }
1377
1378 // -------------------------------------------------------------------
1379 static void
1380 biosfn_read_all_dac_reg(struct bregs *regs)
1381 {
1382     outb(regs->bl, VGAREG_DAC_READ_ADDRESS);
1383     u8 *data = (u8*)(regs->dx + 0);
1384     int count = regs->cx;
1385     while (count) {
1386         SET_FARVAR(regs->es, *data, inb(VGAREG_DAC_DATA));
1387         data++;
1388         SET_FARVAR(regs->es, *data, inb(VGAREG_DAC_DATA));
1389         data++;
1390         SET_FARVAR(regs->es, *data, inb(VGAREG_DAC_DATA));
1391         data++;
1392         count--;
1393     }
1394 }
1395
1396 // -------------------------------------------------------------------
1397 static void
1398 biosfn_set_pel_mask(struct bregs *regs)
1399 {
1400     outb(regs->bl, VGAREG_PEL_MASK);
1401 }
1402
1403 // -------------------------------------------------------------------
1404 static void
1405 biosfn_read_pel_mask(struct bregs *regs)
1406 {
1407     regs->bl = inb(VGAREG_PEL_MASK);
1408 }
1409
1410 // -------------------------------------------------------------------
1411 static void
1412 biosfn_read_video_dac_state(struct bregs *regs)
1413 {
1414     inb(VGAREG_ACTL_RESET);
1415     outb(0x10, VGAREG_ACTL_ADDRESS);
1416     u8 val1 = inb(VGAREG_ACTL_READ_DATA) >> 7;
1417
1418     inb(VGAREG_ACTL_RESET);
1419     outb(0x14, VGAREG_ACTL_ADDRESS);
1420     u8 val2 = inb(VGAREG_ACTL_READ_DATA) & 0x0f;
1421     if (!(val1 & 0x01))
1422         val2 >>= 2;
1423
1424     inb(VGAREG_ACTL_RESET);
1425     outb(0x20, VGAREG_ACTL_ADDRESS);
1426
1427     regs->bl = val1;
1428     regs->bh = val2;
1429 }
1430
1431 // -------------------------------------------------------------------
1432 static void
1433 get_font_access()
1434 {
1435     outw(0x0100, VGAREG_SEQU_ADDRESS);
1436     outw(0x0402, VGAREG_SEQU_ADDRESS);
1437     outw(0x0704, VGAREG_SEQU_ADDRESS);
1438     outw(0x0300, VGAREG_SEQU_ADDRESS);
1439     outw(0x0204, VGAREG_GRDC_ADDRESS);
1440     outw(0x0005, VGAREG_GRDC_ADDRESS);
1441     outw(0x0406, VGAREG_GRDC_ADDRESS);
1442 }
1443
1444 static void
1445 release_font_access()
1446 {
1447     outw(0x0100, VGAREG_SEQU_ADDRESS);
1448     outw(0x0302, VGAREG_SEQU_ADDRESS);
1449     outw(0x0304, VGAREG_SEQU_ADDRESS);
1450     outw(0x0300, VGAREG_SEQU_ADDRESS);
1451     u16 v = inw(VGAREG_READ_MISC_OUTPUT);
1452     v = ((v & 0x01) << 10) | 0x0a06;
1453     outw(v, VGAREG_GRDC_ADDRESS);
1454     outw(0x0004, VGAREG_GRDC_ADDRESS);
1455     outw(0x1005, VGAREG_GRDC_ADDRESS);
1456 }
1457
1458 static void
1459 set_scan_lines(u8 lines)
1460 {
1461     u16 crtc_addr, cols, vde;
1462     u8 crtc_r9, ovl, rows;
1463
1464     crtc_addr = GET_BDA(crtc_address);
1465     outb(0x09, crtc_addr);
1466     crtc_r9 = inb(crtc_addr + 1);
1467     crtc_r9 = (crtc_r9 & 0xe0) | (lines - 1);
1468     outb(crtc_r9, crtc_addr + 1);
1469     if (lines == 8)
1470         biosfn_set_cursor_shape(0x06, 0x07);
1471     else
1472         biosfn_set_cursor_shape(lines - 4, lines - 3);
1473     SET_BDA(char_height, lines);
1474     outb(0x12, crtc_addr);
1475     vde = inb(crtc_addr + 1);
1476     outb(0x07, crtc_addr);
1477     ovl = inb(crtc_addr + 1);
1478     vde += (((ovl & 0x02) << 7) + ((ovl & 0x40) << 3) + 1);
1479     rows = vde / lines;
1480     SET_BDA(video_rows, rows - 1);
1481     cols = GET_BDA(video_cols);
1482     SET_BDA(video_pagesize, rows * cols * 2);
1483 }
1484
1485 static void
1486 biosfn_load_text_user_pat(u8 AL, u16 ES, u16 BP, u16 CX, u16 DX, u8 BL,
1487                           u8 BH)
1488 {
1489     get_font_access();
1490     u16 blockaddr = ((BL & 0x03) << 14) + ((BL & 0x04) << 11);
1491     u16 i;
1492     for (i = 0; i < CX; i++) {
1493         void *src = (void*)(BP + i * BH);
1494         void *dest = (void*)(blockaddr + (DX + i) * 32);
1495         memcpy_far(0xA000, dest, ES, src, BH);
1496     }
1497     release_font_access();
1498     if (AL >= 0x10)
1499         set_scan_lines(BH);
1500 }
1501
1502 static void
1503 biosfn_load_text_8_14_pat(u8 AL, u8 BL)
1504 {
1505     get_font_access();
1506     u16 blockaddr = ((BL & 0x03) << 14) + ((BL & 0x04) << 11);
1507     u16 i;
1508     for (i = 0; i < 0x100; i++) {
1509         u16 src = i * 14;
1510         void *dest = (void*)(blockaddr + i * 32);
1511         memcpy_far(0xA000, dest, 0xC000, &vgafont14[src], 14);
1512     }
1513     release_font_access();
1514     if (AL >= 0x10)
1515         set_scan_lines(14);
1516 }
1517
1518 static void
1519 biosfn_load_text_8_8_pat(u8 AL, u8 BL)
1520 {
1521     get_font_access();
1522     u16 blockaddr = ((BL & 0x03) << 14) + ((BL & 0x04) << 11);
1523     u16 i;
1524     for (i = 0; i < 0x100; i++) {
1525         u16 src = i * 8;
1526         void *dest = (void*)(blockaddr + i * 32);
1527         memcpy_far(0xA000, dest, 0xC000, &vgafont8[src], 8);
1528     }
1529     release_font_access();
1530     if (AL >= 0x10)
1531         set_scan_lines(8);
1532 }
1533
1534 // -------------------------------------------------------------------
1535 static void
1536 biosfn_set_text_block_specifier(struct bregs *regs)
1537 {
1538     outw((regs->bl << 8) | 0x03, VGAREG_SEQU_ADDRESS);
1539 }
1540
1541 // -------------------------------------------------------------------
1542 static void
1543 biosfn_load_text_8_16_pat(u8 AL, u8 BL)
1544 {
1545     get_font_access();
1546     u16 blockaddr = ((BL & 0x03) << 14) + ((BL & 0x04) << 11);
1547     u16 i;
1548     for (i = 0; i < 0x100; i++) {
1549         u16 src = i * 16;
1550         void *dest = (void*)(blockaddr + i * 32);
1551         memcpy_far(0xA000, dest, 0xC000, &vgafont16[src], 16);
1552     }
1553     release_font_access();
1554     if (AL >= 0x10)
1555         set_scan_lines(16);
1556 }
1557
1558 // -------------------------------------------------------------------
1559 static void
1560 biosfn_get_font_info(u8 BH, u16 *ES, u16 *BP, u16 *CX, u16 *DX)
1561 {
1562     switch (BH) {
1563     case 0x00: {
1564         u32 segoff = GET_IVT(0x1f).segoff;
1565         *ES = segoff >> 16;
1566         *BP = segoff;
1567         break;
1568     }
1569     case 0x01: {
1570         u32 segoff = GET_IVT(0x43).segoff;
1571         *ES = segoff >> 16;
1572         *BP = segoff;
1573         break;
1574     }
1575     case 0x02:
1576         *ES = 0xC000;
1577         *BP = (u32)vgafont14;
1578         break;
1579     case 0x03:
1580         *ES = 0xC000;
1581         *BP = (u32)vgafont8;
1582         break;
1583     case 0x04:
1584         *ES = 0xC000;
1585         *BP = (u32)vgafont8 + 128 * 8;
1586         break;
1587     case 0x05:
1588         *ES = 0xC000;
1589         *BP = (u32)vgafont14alt;
1590         break;
1591     case 0x06:
1592         *ES = 0xC000;
1593         *BP = (u32)vgafont16;
1594         break;
1595     case 0x07:
1596         *ES = 0xC000;
1597         *BP = (u32)vgafont16alt;
1598         break;
1599     default:
1600         dprintf(1, "Get font info BH(%02x) was discarded\n", BH);
1601         return;
1602     }
1603     // Set byte/char of on screen font
1604     *CX = GET_BDA(char_height) & 0xff;
1605
1606     // Set Highest char row
1607     *DX = GET_BDA(video_rows);
1608 }
1609
1610 // -------------------------------------------------------------------
1611 static void
1612 biosfn_get_ega_info(struct bregs *regs)
1613 {
1614     regs->cx = GET_BDA(video_switches) & 0x0f;
1615     regs->ax = GET_BDA(crtc_address);
1616     if (regs->ax == VGAREG_MDA_CRTC_ADDRESS)
1617         regs->bx = 0x0103;
1618     else
1619         regs->bx = 0x0003;
1620 }
1621
1622 // -------------------------------------------------------------------
1623 static void
1624 biosfn_select_vert_res(struct bregs *regs)
1625 {
1626     u8 mctl = GET_BDA(modeset_ctl);
1627     u8 vswt = GET_BDA(video_switches);
1628
1629     switch (regs->al) {
1630     case 0x00:
1631         // 200 lines
1632         mctl = (mctl & ~0x10) | 0x80;
1633         vswt = (vswt & ~0x0f) | 0x08;
1634         break;
1635     case 0x01:
1636         // 350 lines
1637         mctl &= ~0x90;
1638         vswt = (vswt & ~0x0f) | 0x09;
1639         break;
1640     case 0x02:
1641         // 400 lines
1642         mctl = (mctl & ~0x80) | 0x10;
1643         vswt = (vswt & ~0x0f) | 0x09;
1644         break;
1645     default:
1646         dprintf(1, "Select vert res (%02x) was discarded\n", regs->al);
1647         break;
1648     }
1649     SET_BDA(modeset_ctl, mctl);
1650     SET_BDA(video_switches, vswt);
1651     regs->ax = 0x1212;
1652 }
1653
1654 static void
1655 biosfn_enable_default_palette_loading(struct bregs *regs)
1656 {
1657     u8 v = (regs->al & 0x01) << 3;
1658     u8 mctl = GET_BDA(video_ctl) & ~0x08;
1659     SET_BDA(video_ctl, mctl | v);
1660     regs->ax = 0x1212;
1661 }
1662
1663 static void
1664 biosfn_enable_video_addressing(struct bregs *regs)
1665 {
1666     u8 v = ((regs->al << 1) & 0x02) ^ 0x02;
1667     u8 v2 = inb(VGAREG_READ_MISC_OUTPUT) & ~0x02;
1668     outb(v | v2, VGAREG_WRITE_MISC_OUTPUT);
1669     regs->ax = 0x1212;
1670 }
1671
1672
1673 static void
1674 biosfn_enable_grayscale_summing(struct bregs *regs)
1675 {
1676     u8 v = ((regs->al << 1) & 0x02) ^ 0x02;
1677     u8 v2 = GET_BDA(modeset_ctl) & ~0x02;
1678     SET_BDA(modeset_ctl, v | v2);
1679     regs->ax = 0x1212;
1680 }
1681
1682 static void
1683 biosfn_enable_cursor_emulation(struct bregs *regs)
1684 {
1685     u8 v = (regs->al & 0x01) ^ 0x01;
1686     u8 v2 = GET_BDA(modeset_ctl) & ~0x01;
1687     SET_BDA(modeset_ctl, v | v2);
1688     regs->ax = 0x1212;
1689 }
1690
1691 // -------------------------------------------------------------------
1692 static void
1693 biosfn_write_string(u8 flag, u8 page, u8 attr, u16 count, u8 row, u8 col,
1694                     u16 seg, u8 *offset)
1695 {
1696     u16 newcurs, oldcurs, dummy;
1697     u8 car;
1698
1699     // Read curs info for the page
1700     biosfn_get_cursor_pos(page, &dummy, &oldcurs);
1701
1702     // if row=0xff special case : use current cursor position
1703     if (row == 0xff) {
1704         col = oldcurs & 0x00ff;
1705         row = (oldcurs & 0xff00) >> 8;
1706     }
1707
1708     newcurs = row;
1709     newcurs <<= 8;
1710     newcurs += col;
1711     biosfn_set_cursor_pos(page, newcurs);
1712
1713     while (count-- != 0) {
1714         car = GET_FARVAR(seg, *offset);
1715         offset++;
1716         if ((flag & 0x02) != 0) {
1717             attr = GET_FARVAR(seg, *offset);
1718             offset++;
1719         }
1720
1721         biosfn_write_teletype(car, page, attr, WITH_ATTR);
1722     }
1723
1724     // Set back curs pos
1725     if ((flag & 0x01) == 0)
1726         biosfn_set_cursor_pos(page, oldcurs);
1727 }
1728
1729 // -------------------------------------------------------------------
1730 static void
1731 biosfn_read_display_code(struct bregs *regs)
1732 {
1733     regs->bx = GET_BDA(dcc_index);
1734     regs->al = 0x1a;
1735 }
1736
1737 static void
1738 biosfn_set_display_code(struct bregs *regs)
1739 {
1740     SET_BDA(dcc_index, regs->bl);
1741     dprintf(1, "Alternate Display code (%02x) was discarded\n", regs->bh);
1742     regs->al = 0x1a;
1743 }
1744
1745 // -------------------------------------------------------------------
1746 static void
1747 biosfn_read_state_info(u16 BX, u16 ES, u16 DI)
1748 {
1749     // Address of static functionality table
1750     SET_FARVAR(ES, *(u16*)(DI + 0x00), (u32)static_functionality);
1751     SET_FARVAR(ES, *(u16*)(DI + 0x02), 0xC000);
1752
1753     // Hard coded copy from BIOS area. Should it be cleaner ?
1754     memcpy_far(ES, (void*)(DI + 0x04), SEG_BDA, (void*)0x49, 30);
1755     memcpy_far(ES, (void*)(DI + 0x22), SEG_BDA, (void*)0x84, 3);
1756
1757     SET_FARVAR(ES, *(u8*)(DI + 0x25), GET_BDA(dcc_index));
1758     SET_FARVAR(ES, *(u8*)(DI + 0x26), 0);
1759     SET_FARVAR(ES, *(u8*)(DI + 0x27), 16);
1760     SET_FARVAR(ES, *(u8*)(DI + 0x28), 0);
1761     SET_FARVAR(ES, *(u8*)(DI + 0x29), 8);
1762     SET_FARVAR(ES, *(u8*)(DI + 0x2a), 2);
1763     SET_FARVAR(ES, *(u8*)(DI + 0x2b), 0);
1764     SET_FARVAR(ES, *(u8*)(DI + 0x2c), 0);
1765     SET_FARVAR(ES, *(u8*)(DI + 0x31), 3);
1766     SET_FARVAR(ES, *(u8*)(DI + 0x32), 0);
1767
1768     memset_far(ES, (void*)(DI + 0x33), 0, 13);
1769 }
1770
1771 // -------------------------------------------------------------------
1772 // -------------------------------------------------------------------
1773 static u16
1774 biosfn_read_video_state_size(u16 CX)
1775 {
1776     u16 size = 0;
1777     if (CX & 1)
1778         size += 0x46;
1779     if (CX & 2)
1780         size += (5 + 8 + 5) * 2 + 6;
1781     if (CX & 4)
1782         size += 3 + 256 * 3 + 1;
1783     return size;
1784 }
1785
1786 static u16
1787 biosfn_save_video_state(u16 CX, u16 ES, u16 BX)
1788 {
1789     u16 i, crtc_addr, ar_index;
1790
1791     crtc_addr = GET_BDA(crtc_address);
1792     if (CX & 1) {
1793         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_ADDRESS));
1794         BX++;
1795         SET_FARVAR(ES, *(u8*)(BX+0), inb(crtc_addr));
1796         BX++;
1797         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_GRDC_ADDRESS));
1798         BX++;
1799         inb(VGAREG_ACTL_RESET);
1800         ar_index = inb(VGAREG_ACTL_ADDRESS);
1801         SET_FARVAR(ES, *(u8*)(BX+0), ar_index);
1802         BX++;
1803         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_READ_FEATURE_CTL));
1804         BX++;
1805
1806         for (i = 1; i <= 4; i++) {
1807             outb(i, VGAREG_SEQU_ADDRESS);
1808             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_DATA));
1809             BX++;
1810         }
1811         outb(0, VGAREG_SEQU_ADDRESS);
1812         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_DATA));
1813         BX++;
1814
1815         for (i = 0; i <= 0x18; i++) {
1816             outb(i, crtc_addr);
1817             SET_FARVAR(ES, *(u8*)(BX+0), inb(crtc_addr + 1));
1818             BX++;
1819         }
1820
1821         for (i = 0; i <= 0x13; i++) {
1822             inb(VGAREG_ACTL_RESET);
1823             outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
1824             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_ACTL_READ_DATA));
1825             BX++;
1826         }
1827         inb(VGAREG_ACTL_RESET);
1828
1829         for (i = 0; i <= 8; i++) {
1830             outb(i, VGAREG_GRDC_ADDRESS);
1831             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_GRDC_DATA));
1832             BX++;
1833         }
1834
1835         SET_FARVAR(ES, *(u16*)(BX+0), crtc_addr);
1836         BX += 2;
1837
1838         /* XXX: read plane latches */
1839         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1840         BX++;
1841         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1842         BX++;
1843         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1844         BX++;
1845         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1846         BX++;
1847     }
1848     if (CX & 2) {
1849         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_mode));
1850         BX++;
1851         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_cols));
1852         BX += 2;
1853         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_pagesize));
1854         BX += 2;
1855         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(crtc_address));
1856         BX += 2;
1857         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_rows));
1858         BX++;
1859         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(char_height));
1860         BX += 2;
1861         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_ctl));
1862         BX++;
1863         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_switches));
1864         BX++;
1865         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(modeset_ctl));
1866         BX++;
1867         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(cursor_type));
1868         BX += 2;
1869         for (i = 0; i < 8; i++) {
1870             SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(cursor_pos[i]));
1871             BX += 2;
1872         }
1873         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_pagestart));
1874         BX += 2;
1875         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_page));
1876         BX++;
1877         /* current font */
1878         SET_FARVAR(ES, *(u16*)(BX+0), GET_FARVAR(0, *(u16*)(0x1f * 4)));
1879         BX += 2;
1880         SET_FARVAR(ES, *(u16*)(BX+0), GET_FARVAR(0, *(u16*)(0x1f * 4 + 2)));
1881         BX += 2;
1882         SET_FARVAR(ES, *(u16*)(BX+0), GET_FARVAR(0, *(u16*)(0x43 * 4)));
1883         BX += 2;
1884         SET_FARVAR(ES, *(u16*)(BX+0), GET_FARVAR(0, *(u16*)(0x43 * 4 + 2)));
1885         BX += 2;
1886     }
1887     if (CX & 4) {
1888         /* XXX: check this */
1889         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_STATE));
1890         BX++;                   /* read/write mode dac */
1891         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_WRITE_ADDRESS));
1892         BX++;                   /* pix address */
1893         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_PEL_MASK));
1894         BX++;
1895         // Set the whole dac always, from 0
1896         outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
1897         for (i = 0; i < 256 * 3; i++) {
1898             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_DATA));
1899             BX++;
1900         }
1901         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1902         BX++;                   /* color select register */
1903     }
1904     return BX;
1905 }
1906
1907 static u16
1908 biosfn_restore_video_state(u16 CX, u16 ES, u16 BX)
1909 {
1910     u16 i, crtc_addr, v, addr1, ar_index;
1911
1912     if (CX & 1) {
1913         // Reset Attribute Ctl flip-flop
1914         inb(VGAREG_ACTL_RESET);
1915
1916         crtc_addr = GET_FARVAR(ES, *(u16*)(BX + 0x40));
1917         addr1 = BX;
1918         BX += 5;
1919
1920         for (i = 1; i <= 4; i++) {
1921             outb(i, VGAREG_SEQU_ADDRESS);
1922             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_SEQU_DATA);
1923             BX++;
1924         }
1925         outb(0, VGAREG_SEQU_ADDRESS);
1926         outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_SEQU_DATA);
1927         BX++;
1928
1929         // Disable CRTC write protection
1930         outw(0x0011, crtc_addr);
1931         // Set CRTC regs
1932         for (i = 0; i <= 0x18; i++) {
1933             if (i != 0x11) {
1934                 outb(i, crtc_addr);
1935                 outb(GET_FARVAR(ES, *(u8*)(BX+0)), crtc_addr + 1);
1936             }
1937             BX++;
1938         }
1939         // select crtc base address
1940         v = inb(VGAREG_READ_MISC_OUTPUT) & ~0x01;
1941         if (crtc_addr == VGAREG_VGA_CRTC_ADDRESS)
1942             v |= 0x01;
1943         outb(v, VGAREG_WRITE_MISC_OUTPUT);
1944
1945         // enable write protection if needed
1946         outb(0x11, crtc_addr);
1947         outb(GET_FARVAR(ES, *(u8*)(BX - 0x18 + 0x11)), crtc_addr + 1);
1948
1949         // Set Attribute Ctl
1950         ar_index = GET_FARVAR(ES, *(u8*)(addr1 + 0x03));
1951         inb(VGAREG_ACTL_RESET);
1952         for (i = 0; i <= 0x13; i++) {
1953             outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
1954             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_ACTL_WRITE_DATA);
1955             BX++;
1956         }
1957         outb(ar_index, VGAREG_ACTL_ADDRESS);
1958         inb(VGAREG_ACTL_RESET);
1959
1960         for (i = 0; i <= 8; i++) {
1961             outb(i, VGAREG_GRDC_ADDRESS);
1962             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_GRDC_DATA);
1963             BX++;
1964         }
1965         BX += 2;                /* crtc_addr */
1966         BX += 4;                /* plane latches */
1967
1968         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), VGAREG_SEQU_ADDRESS);
1969         addr1++;
1970         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), crtc_addr);
1971         addr1++;
1972         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), VGAREG_GRDC_ADDRESS);
1973         addr1++;
1974         addr1++;
1975         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), crtc_addr - 0x4 + 0xa);
1976         addr1++;
1977     }
1978     if (CX & 2) {
1979         SET_BDA(video_mode, GET_FARVAR(ES, *(u8*)(BX+0)));
1980         BX++;
1981         SET_BDA(video_cols, GET_FARVAR(ES, *(u16*)(BX+0)));
1982         BX += 2;
1983         SET_BDA(video_pagesize, GET_FARVAR(ES, *(u16*)(BX+0)));
1984         BX += 2;
1985         SET_BDA(crtc_address, GET_FARVAR(ES, *(u16*)(BX+0)));
1986         BX += 2;
1987         SET_BDA(video_rows, GET_FARVAR(ES, *(u8*)(BX+0)));
1988         BX++;
1989         SET_BDA(char_height, GET_FARVAR(ES, *(u16*)(BX+0)));
1990         BX += 2;
1991         SET_BDA(video_ctl, GET_FARVAR(ES, *(u8*)(BX+0)));
1992         BX++;
1993         SET_BDA(video_switches, GET_FARVAR(ES, *(u8*)(BX+0)));
1994         BX++;
1995         SET_BDA(modeset_ctl, GET_FARVAR(ES, *(u8*)(BX+0)));
1996         BX++;
1997         SET_BDA(cursor_type, GET_FARVAR(ES, *(u16*)(BX+0)));
1998         BX += 2;
1999         for (i = 0; i < 8; i++) {
2000             SET_BDA(cursor_pos[i], GET_FARVAR(ES, *(u16*)(BX+0)));
2001             BX += 2;
2002         }
2003         SET_BDA(video_pagestart, GET_FARVAR(ES, *(u16*)(BX+0)));
2004         BX += 2;
2005         SET_BDA(video_page, GET_FARVAR(ES, *(u8*)(BX+0)));
2006         BX++;
2007         /* current font */
2008         SET_IVT(0x1f, GET_FARVAR(ES, *(u16*)(BX+2)), GET_FARVAR(ES, *(u16*)(BX+0)));
2009         BX += 4;
2010         SET_IVT(0x43, GET_FARVAR(ES, *(u16*)(BX+2)), GET_FARVAR(ES, *(u16*)(BX+0)));
2011         BX += 4;
2012     }
2013     if (CX & 4) {
2014         BX++;
2015         v = GET_FARVAR(ES, *(u8*)(BX+0));
2016         BX++;
2017         outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_PEL_MASK);
2018         BX++;
2019         // Set the whole dac always, from 0
2020         outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
2021         for (i = 0; i < 256 * 3; i++) {
2022             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_DAC_DATA);
2023             BX++;
2024         }
2025         BX++;
2026         outb(v, VGAREG_DAC_WRITE_ADDRESS);
2027     }
2028     return BX;
2029 }
2030
2031
2032 /****************************************************************
2033  * VGA int 10 handler
2034  ****************************************************************/
2035
2036 static void
2037 handle_1000(struct bregs *regs)
2038 {
2039     // XXX - inline
2040     biosfn_set_video_mode(regs->al);
2041     switch(regs->al & 0x7F) {
2042     case 6:
2043         regs->al = 0x3F;
2044         break;
2045     case 0:
2046     case 1:
2047     case 2:
2048     case 3:
2049     case 4:
2050     case 5:
2051     case 7:
2052         regs->al = 0x30;
2053         break;
2054     default:
2055         regs->al = 0x20;
2056     }
2057 }
2058
2059 static void
2060 handle_1001(struct bregs *regs)
2061 {
2062     biosfn_set_cursor_shape(regs->ch, regs->cl);
2063 }
2064
2065 static void
2066 handle_1002(struct bregs *regs)
2067 {
2068     biosfn_set_cursor_pos(regs->bh, regs->dx);
2069 }
2070
2071 static void
2072 handle_1003(struct bregs *regs)
2073 {
2074     biosfn_get_cursor_pos(regs->bh, &regs->cx, &regs->dx);
2075 }
2076
2077 // Read light pen pos (unimplemented)
2078 static void
2079 handle_1004(struct bregs *regs)
2080 {
2081     debug_stub(regs);
2082     regs->ax = regs->bx = regs->cx = regs->dx = 0;
2083 }
2084
2085 static void
2086 handle_1005(struct bregs *regs)
2087 {
2088     biosfn_set_active_page(regs->al);
2089 }
2090
2091 static void
2092 handle_1006(struct bregs *regs)
2093 {
2094     biosfn_scroll(regs->al, regs->bh, regs->ch, regs->cl, regs->dh, regs->dl
2095                   , 0xFF, SCROLL_UP);
2096 }
2097
2098 static void
2099 handle_1007(struct bregs *regs)
2100 {
2101     biosfn_scroll(regs->al, regs->bh, regs->ch, regs->cl, regs->dh, regs->dl
2102                   , 0xFF, SCROLL_DOWN);
2103 }
2104
2105 static void
2106 handle_1008(struct bregs *regs)
2107 {
2108     // XXX - inline
2109     biosfn_read_char_attr(regs->bh, &regs->ax);
2110 }
2111
2112 static void
2113 handle_1009(struct bregs *regs)
2114 {
2115     // XXX - inline
2116     biosfn_write_char_attr(regs->al, regs->bh, regs->bl, regs->cx);
2117 }
2118
2119 static void
2120 handle_100a(struct bregs *regs)
2121 {
2122     // XXX - inline
2123     biosfn_write_char_only(regs->al, regs->bh, regs->bl, regs->cx);
2124 }
2125
2126
2127 static void
2128 handle_100b00(struct bregs *regs)
2129 {
2130     // XXX - inline
2131     biosfn_set_border_color(regs);
2132 }
2133
2134 static void
2135 handle_100b01(struct bregs *regs)
2136 {
2137     // XXX - inline
2138     biosfn_set_palette(regs);
2139 }
2140
2141 static void
2142 handle_100bXX(struct bregs *regs)
2143 {
2144     debug_stub(regs);
2145 }
2146
2147 static void
2148 handle_100b(struct bregs *regs)
2149 {
2150     switch (regs->bh) {
2151     case 0x00: handle_100b00(regs); break;
2152     case 0x01: handle_100b01(regs); break;
2153     default:   handle_100bXX(regs); break;
2154     }
2155 }
2156
2157
2158 static void
2159 handle_100c(struct bregs *regs)
2160 {
2161     // XXX - inline
2162     biosfn_write_pixel(regs->bh, regs->al, regs->cx, regs->dx);
2163 }
2164
2165 static void
2166 handle_100d(struct bregs *regs)
2167 {
2168     // XXX - inline
2169     biosfn_read_pixel(regs->bh, regs->cx, regs->dx, &regs->ax);
2170 }
2171
2172 static void
2173 handle_100e(struct bregs *regs)
2174 {
2175     // Ralf Brown Interrupt list is WRONG on bh(page)
2176     // We do output only on the current page !
2177     biosfn_write_teletype(regs->al, 0xff, regs->bl, NO_ATTR);
2178 }
2179
2180 static void
2181 handle_100f(struct bregs *regs)
2182 {
2183     // XXX - inline
2184     biosfn_get_video_mode(regs);
2185 }
2186
2187
2188 static void
2189 handle_101000(struct bregs *regs)
2190 {
2191     if (regs->bl > 0x14)
2192         return;
2193     biosfn_set_single_palette_reg(regs->bl, regs->bh);
2194 }
2195
2196 static void
2197 handle_101001(struct bregs *regs)
2198 {
2199     // XXX - inline
2200     biosfn_set_overscan_border_color(regs);
2201 }
2202
2203 static void
2204 handle_101002(struct bregs *regs)
2205 {
2206     // XXX - inline
2207     biosfn_set_all_palette_reg(regs);
2208 }
2209
2210 static void
2211 handle_101003(struct bregs *regs)
2212 {
2213     // XXX - inline
2214     biosfn_toggle_intensity(regs);
2215 }
2216
2217 static void
2218 handle_101007(struct bregs *regs)
2219 {
2220     if (regs->bl > 0x14)
2221         return;
2222     regs->bh = biosfn_get_single_palette_reg(regs->bl);
2223 }
2224
2225 static void
2226 handle_101008(struct bregs *regs)
2227 {
2228     // XXX - inline
2229     biosfn_read_overscan_border_color(regs);
2230 }
2231
2232 static void
2233 handle_101009(struct bregs *regs)
2234 {
2235     // XXX - inline
2236     biosfn_get_all_palette_reg(regs);
2237 }
2238
2239 static void
2240 handle_101010(struct bregs *regs)
2241 {
2242     // XXX - inline
2243     biosfn_set_single_dac_reg(regs);
2244 }
2245
2246 static void
2247 handle_101012(struct bregs *regs)
2248 {
2249     // XXX - inline
2250     biosfn_set_all_dac_reg(regs);
2251 }
2252
2253 static void
2254 handle_101013(struct bregs *regs)
2255 {
2256     // XXX - inline
2257     biosfn_select_video_dac_color_page(regs);
2258 }
2259
2260 static void
2261 handle_101015(struct bregs *regs)
2262 {
2263     // XXX - inline
2264     biosfn_read_single_dac_reg(regs);
2265 }
2266
2267 static void
2268 handle_101017(struct bregs *regs)
2269 {
2270     // XXX - inline
2271     biosfn_read_all_dac_reg(regs);
2272 }
2273
2274 static void
2275 handle_101018(struct bregs *regs)
2276 {
2277     // XXX - inline
2278     biosfn_set_pel_mask(regs);
2279 }
2280
2281 static void
2282 handle_101019(struct bregs *regs)
2283 {
2284     // XXX - inline
2285     biosfn_read_pel_mask(regs);
2286 }
2287
2288 static void
2289 handle_10101a(struct bregs *regs)
2290 {
2291     // XXX - inline
2292     biosfn_read_video_dac_state(regs);
2293 }
2294
2295 static void
2296 handle_10101b(struct bregs *regs)
2297 {
2298     biosfn_perform_gray_scale_summing(regs->bx, regs->cx);
2299 }
2300
2301 static void
2302 handle_1010XX(struct bregs *regs)
2303 {
2304     debug_stub(regs);
2305 }
2306
2307 static void
2308 handle_1010(struct bregs *regs)
2309 {
2310     switch (regs->al) {
2311     case 0x00: handle_101000(regs); break;
2312     case 0x01: handle_101001(regs); break;
2313     case 0x02: handle_101002(regs); break;
2314     case 0x03: handle_101003(regs); break;
2315     case 0x07: handle_101007(regs); break;
2316     case 0x08: handle_101008(regs); break;
2317     case 0x09: handle_101009(regs); break;
2318     case 0x10: handle_101010(regs); break;
2319     case 0x12: handle_101012(regs); break;
2320     case 0x13: handle_101013(regs); break;
2321     case 0x15: handle_101015(regs); break;
2322     case 0x17: handle_101017(regs); break;
2323     case 0x18: handle_101018(regs); break;
2324     case 0x19: handle_101019(regs); break;
2325     case 0x1a: handle_10101a(regs); break;
2326     case 0x1b: handle_10101b(regs); break;
2327     default:   handle_1010XX(regs); break;
2328     }
2329 }
2330
2331
2332 static void
2333 handle_101100(struct bregs *regs)
2334 {
2335     // XXX - inline
2336     biosfn_load_text_user_pat(regs->al, regs->es, 0 // XXX - regs->bp
2337                               , regs->cx, regs->dx, regs->bl, regs->bh);
2338 }
2339
2340 static void
2341 handle_101101(struct bregs *regs)
2342 {
2343     // XXX - inline
2344     biosfn_load_text_8_14_pat(regs->al, regs->bl);
2345 }
2346
2347 static void
2348 handle_101102(struct bregs *regs)
2349 {
2350     // XXX - inline
2351     biosfn_load_text_8_8_pat(regs->al, regs->bl);
2352 }
2353
2354 static void
2355 handle_101103(struct bregs *regs)
2356 {
2357     // XXX - inline
2358     biosfn_set_text_block_specifier(regs);
2359 }
2360
2361 static void
2362 handle_101104(struct bregs *regs)
2363 {
2364     // XXX - inline
2365     biosfn_load_text_8_16_pat(regs->al, regs->bl);
2366 }
2367
2368 static void
2369 handle_101110(struct bregs *regs)
2370 {
2371     handle_101100(regs);
2372 }
2373
2374 static void
2375 handle_101111(struct bregs *regs)
2376 {
2377     handle_101101(regs);
2378 }
2379
2380 static void
2381 handle_101112(struct bregs *regs)
2382 {
2383     handle_101102(regs);
2384 }
2385
2386 static void
2387 handle_101114(struct bregs *regs)
2388 {
2389     handle_101104(regs);
2390 }
2391
2392 static void
2393 handle_101130(struct bregs *regs)
2394 {
2395     // XXX - inline
2396     biosfn_get_font_info(regs->bh, &regs->es, 0 // &regs->bp
2397                          , &regs->cx, &regs->dx);
2398 }
2399
2400 static void
2401 handle_1011XX(struct bregs *regs)
2402 {
2403     debug_stub(regs);
2404 }
2405
2406 static void
2407 handle_1011(struct bregs *regs)
2408 {
2409     switch (regs->al) {
2410     case 0x00: handle_101100(regs); break;
2411     case 0x01: handle_101101(regs); break;
2412     case 0x02: handle_101102(regs); break;
2413     case 0x03: handle_101103(regs); break;
2414     case 0x04: handle_101104(regs); break;
2415     case 0x10: handle_101110(regs); break;
2416     case 0x11: handle_101111(regs); break;
2417     case 0x12: handle_101112(regs); break;
2418     case 0x14: handle_101114(regs); break;
2419     case 0x30: handle_101130(regs); break;
2420     default:   handle_1011XX(regs); break;
2421     }
2422 }
2423
2424
2425 static void
2426 handle_101210(struct bregs *regs)
2427 {
2428     // XXX - inline
2429     biosfn_get_ega_info(regs);
2430 }
2431
2432 static void
2433 handle_101230(struct bregs *regs)
2434 {
2435     // XXX - inline
2436     biosfn_select_vert_res(regs);
2437 }
2438
2439 static void
2440 handle_101231(struct bregs *regs)
2441 {
2442     // XXX - inline
2443     biosfn_enable_default_palette_loading(regs);
2444 }
2445
2446 static void
2447 handle_101232(struct bregs *regs)
2448 {
2449     // XXX - inline
2450     biosfn_enable_video_addressing(regs);
2451 }
2452
2453 static void
2454 handle_101233(struct bregs *regs)
2455 {
2456     // XXX - inline
2457     biosfn_enable_grayscale_summing(regs);
2458 }
2459
2460 static void
2461 handle_101234(struct bregs *regs)
2462 {
2463     // XXX - inline
2464     biosfn_enable_cursor_emulation(regs);
2465 }
2466
2467 static void
2468 handle_101235(struct bregs *regs)
2469 {
2470     debug_stub(regs);
2471     regs->al = 0x12;
2472 }
2473
2474 static void
2475 handle_101236(struct bregs *regs)
2476 {
2477     debug_stub(regs);
2478     regs->al = 0x12;
2479 }
2480
2481 static void
2482 handle_1012XX(struct bregs *regs)
2483 {
2484     debug_stub(regs);
2485 }
2486
2487 static void
2488 handle_1012(struct bregs *regs)
2489 {
2490     switch (regs->bl) {
2491     case 0x10: handle_101210(regs); break;
2492     case 0x30: handle_101230(regs); break;
2493     case 0x31: handle_101231(regs); break;
2494     case 0x32: handle_101232(regs); break;
2495     case 0x33: handle_101233(regs); break;
2496     case 0x34: handle_101234(regs); break;
2497     case 0x35: handle_101235(regs); break;
2498     case 0x36: handle_101236(regs); break;
2499     default:   handle_1012XX(regs); break;
2500     }
2501
2502     // XXX - cirrus has 1280, 1281, 1282, 1285, 129a, 12a0, 12a1, 12a2, 12ae
2503 }
2504
2505
2506 static void
2507 handle_1013(struct bregs *regs)
2508 {
2509     // XXX - inline
2510     biosfn_write_string(regs->al, regs->bh, regs->bl, regs->cx
2511                         , regs->dh, regs->dl, regs->es, 0); // regs->bp);
2512 }
2513
2514
2515 static void
2516 handle_101a00(struct bregs *regs)
2517 {
2518     // XXX - inline
2519     biosfn_read_display_code(regs);
2520 }
2521
2522 static void
2523 handle_101a01(struct bregs *regs)
2524 {
2525     // XXX - inline
2526     biosfn_set_display_code(regs);
2527 }
2528
2529 static void
2530 handle_101aXX(struct bregs *regs)
2531 {
2532     debug_stub(regs);
2533 }
2534
2535 static void
2536 handle_101a(struct bregs *regs)
2537 {
2538     switch (regs->al) {
2539     case 0x00: handle_101a00(regs); break;
2540     case 0x01: handle_101a01(regs); break;
2541     default:   handle_101aXX(regs); break;
2542     }
2543 }
2544
2545
2546 static void
2547 handle_101b(struct bregs *regs)
2548 {
2549     // XXX - inline
2550     biosfn_read_state_info(regs->bx, regs->es, regs->di);
2551     regs->al = 0x1B;
2552 }
2553
2554
2555 static void
2556 handle_101c00(struct bregs *regs)
2557 {
2558     // XXX - inline
2559     regs->bx = biosfn_read_video_state_size(regs->cx);
2560 }
2561
2562 static void
2563 handle_101c01(struct bregs *regs)
2564 {
2565     // XXX - inline
2566     biosfn_save_video_state(regs->cx, regs->es, regs->bx);
2567 }
2568
2569 static void
2570 handle_101c02(struct bregs *regs)
2571 {
2572     // XXX - inline
2573     biosfn_restore_video_state(regs->cx, regs->es, regs->bx);
2574 }
2575
2576 static void
2577 handle_101cXX(struct bregs *regs)
2578 {
2579     debug_stub(regs);
2580 }
2581
2582 static void
2583 handle_101c(struct bregs *regs)
2584 {
2585     switch (regs->al) {
2586     case 0x00: handle_101c00(regs); break;
2587     case 0x01: handle_101c01(regs); break;
2588     case 0x02: handle_101c02(regs); break;
2589     default:   handle_101cXX(regs); break;
2590     }
2591 }
2592
2593
2594 static void
2595 handle_104f00(struct bregs *regs)
2596 {
2597     // XXX - vbe_biosfn_return_controller_information(&AX,ES,DI);
2598     // XXX - OR cirrus_vesa_00h
2599 }
2600
2601 static void
2602 handle_104f01(struct bregs *regs)
2603 {
2604     // XXX - vbe_biosfn_return_mode_information(&AX,CX,ES,DI);
2605     // XXX - OR cirrus_vesa_01h
2606 }
2607
2608 static void
2609 handle_104f02(struct bregs *regs)
2610 {
2611     // XXX - vbe_biosfn_set_mode(&AX,BX,ES,DI);
2612     // XXX - OR cirrus_vesa_02h
2613 }
2614
2615 static void
2616 handle_104f03(struct bregs *regs)
2617 {
2618     // XXX - vbe_biosfn_return_current_mode
2619     // XXX - OR cirrus_vesa_03h
2620 }
2621
2622 static void
2623 handle_104f04(struct bregs *regs)
2624 {
2625     // XXX - vbe_biosfn_save_restore_state(&AX, CX, DX, ES, &BX);
2626 }
2627
2628 static void
2629 handle_104f05(struct bregs *regs)
2630 {
2631     // XXX - vbe_biosfn_display_window_control
2632     // XXX - OR cirrus_vesa_05h
2633 }
2634
2635 static void
2636 handle_104f06(struct bregs *regs)
2637 {
2638     // XXX - vbe_biosfn_set_get_logical_scan_line_length
2639     // XXX - OR cirrus_vesa_06h
2640 }
2641
2642 static void
2643 handle_104f07(struct bregs *regs)
2644 {
2645     // XXX - vbe_biosfn_set_get_display_start
2646     // XXX - OR cirrus_vesa_07h
2647 }
2648
2649 static void
2650 handle_104f08(struct bregs *regs)
2651 {
2652     // XXX - vbe_biosfn_set_get_dac_palette_format
2653 }
2654
2655 static void
2656 handle_104f0a(struct bregs *regs)
2657 {
2658     // XXX - vbe_biosfn_return_protected_mode_interface
2659 }
2660
2661 static void
2662 handle_104fXX(struct bregs *regs)
2663 {
2664     debug_stub(regs);
2665     regs->ax = 0x0100;
2666 }
2667
2668 static void
2669 handle_104f(struct bregs *regs)
2670 {
2671     if (! CONFIG_VBE) {
2672         handle_104fXX(regs);
2673         return;
2674     }
2675
2676     // XXX - check vbe_has_vbe_display()?
2677
2678     switch (regs->al) {
2679     case 0x00: handle_104f00(regs); break;
2680     case 0x01: handle_104f01(regs); break;
2681     case 0x02: handle_104f02(regs); break;
2682     case 0x03: handle_104f03(regs); break;
2683     case 0x04: handle_104f04(regs); break;
2684     case 0x05: handle_104f05(regs); break;
2685     case 0x06: handle_104f06(regs); break;
2686     case 0x07: handle_104f07(regs); break;
2687     case 0x08: handle_104f08(regs); break;
2688     case 0x0a: handle_104f0a(regs); break;
2689     default:   handle_104fXX(regs); break;
2690     }
2691 }
2692
2693
2694 static void
2695 handle_10XX(struct bregs *regs)
2696 {
2697     debug_stub(regs);
2698 }
2699
2700 // INT 10h Video Support Service Entry Point
2701 void VISIBLE16
2702 handle_10(struct bregs *regs)
2703 {
2704     debug_enter(regs, DEBUG_VGA_10);
2705     switch (regs->ah) {
2706     case 0x00: handle_1000(regs); break;
2707     case 0x01: handle_1001(regs); break;
2708     case 0x02: handle_1002(regs); break;
2709     case 0x03: handle_1003(regs); break;
2710     case 0x04: handle_1004(regs); break;
2711     case 0x05: handle_1005(regs); break;
2712     case 0x06: handle_1006(regs); break;
2713     case 0x07: handle_1007(regs); break;
2714     case 0x08: handle_1008(regs); break;
2715     case 0x09: handle_1009(regs); break;
2716     case 0x0a: handle_100a(regs); break;
2717     case 0x0b: handle_100b(regs); break;
2718     case 0x0c: handle_100c(regs); break;
2719     case 0x0d: handle_100d(regs); break;
2720     case 0x0e: handle_100e(regs); break;
2721     case 0x0f: handle_100f(regs); break;
2722     case 0x10: handle_1010(regs); break;
2723     case 0x11: handle_1011(regs); break;
2724     case 0x12: handle_1012(regs); break;
2725     case 0x13: handle_1013(regs); break;
2726     case 0x1a: handle_101a(regs); break;
2727     case 0x1b: handle_101b(regs); break;
2728     case 0x1c: handle_101c(regs); break;
2729     case 0x4f: handle_104f(regs); break;
2730     default:   handle_10XX(regs); break;
2731     }
2732 }
2733
2734
2735 /****************************************************************
2736  * VGA post
2737  ****************************************************************/
2738
2739 static void
2740 init_bios_area()
2741 {
2742     // init detected hardware BIOS Area
2743     // set 80x25 color (not clear from RBIL but usual)
2744     u16 eqf = GET_BDA(equipment_list_flags);
2745     SET_BDA(equipment_list_flags, (eqf & 0xffcf) | 0x20);
2746
2747     // Just for the first int10 find its children
2748
2749     // the default char height
2750     SET_BDA(char_height, 0x10);
2751
2752     // Clear the screen
2753     SET_BDA(video_ctl, 0x60);
2754
2755     // Set the basic screen we have
2756     SET_BDA(video_switches, 0xf9);
2757
2758     // Set the basic modeset options
2759     SET_BDA(modeset_ctl, 0x51);
2760
2761     // Set the  default MSR
2762     SET_BDA(video_msr, 0x09);
2763 }
2764
2765 static void
2766 init_vga_card()
2767 {
2768     // switch to color mode and enable CPU access 480 lines
2769     outb(0xc3, VGAREG_WRITE_MISC_OUTPUT);
2770     // more than 64k 3C4/04
2771     outb(0x04, VGAREG_SEQU_ADDRESS);
2772     outb(0x02, VGAREG_SEQU_DATA);
2773 }
2774
2775 void VISIBLE16
2776 vga_post(struct bregs *regs)
2777 {
2778     debug_enter(regs, DEBUG_VGA_POST);
2779
2780     init_vga_card();
2781
2782     init_bios_area();
2783
2784     // vbe_init();
2785
2786     extern void entry_10(void);
2787     SET_IVT(0x10, 0xC000, (u32)entry_10);
2788
2789     if (CONFIG_CIRRUS)
2790         cirrus_init();
2791
2792     // XXX - clear screen and display info
2793
2794     // XXX: fill it
2795     SET_VGA(video_save_pointer_table[0], (u32)video_param_table);
2796     SET_VGA(video_save_pointer_table[1], 0xC000);
2797
2798     // Fixup checksum
2799     extern u8 _rom_header_size, _rom_header_checksum;
2800     SET_VGA(_rom_header_checksum, 0);
2801     u8 sum = -checksum_far(0xC000, 0, _rom_header_size * 512);
2802     SET_VGA(_rom_header_checksum, sum);
2803 }