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