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, 32*1024);
 394         } else {
 395             if (mode < 0x0d) {
 396                 memset16_far(GET_GLOBAL(vga_modes[line].sstart)
 397                              , 0, 0x0000, 32*1024);
 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, 64*1024);
 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(3, "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 * 2);
 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 * 2);
 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 * 2);
 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 * 2);
 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 * 2);
 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
 862                      , count * 2);
 863     } else {
 864         // FIXME gfx mode not complete
 865         cheight = GET_GLOBAL(video_param_table[GET_GLOBAL(line_to_vpti[line])].cheight);
 866         bpp = GET_GLOBAL(vga_modes[line].pixbits);
 867         while ((count-- > 0) && (xcurs < nbcols)) {
 868             switch (GET_GLOBAL(vga_modes[line].memmodel)) {
 869             case PLANAR4:
 870             case PLANAR1:
 871                 write_gfx_char_pl4(car, attr, xcurs, ycurs, nbcols,
 872                                    cheight);
 873                 break;
 874             case CGA:
 875                 write_gfx_char_cga(car, attr, xcurs, ycurs, nbcols, bpp);
 876                 break;
 877             case LINEAR8:
 878                 write_gfx_char_lin(car, attr, xcurs, ycurs, nbcols);
 879                 break;
 880 #ifdef DEBUG
 881             default:
 882                 unimplemented();
 883 #endif
 884             }
 885             xcurs++;
 886         }
 887     }
 888 }
 889
 890 // -------------------------------------------------------------------
 891 static void
 892 biosfn_write_char_only(u8 car, u8 page, u8 attr, u16 count)
 893 {
 894     u8 cheight, xcurs, ycurs, mode, line, bpp;
 895     u16 nbcols, nbrows;
 896     u16 cursor, dummy;
 897
 898     // Get the mode
 899     mode = GET_BDA(video_mode);
 900     line = find_vga_entry(mode);
 901     if (line == 0xFF)
 902         return;
 903
 904     // Get the cursor pos for the page
 905     biosfn_get_cursor_pos(page, &dummy, &cursor);
 906     xcurs = cursor & 0x00ff;
 907     ycurs = (cursor & 0xff00) >> 8;
 908
 909     // Get the dimensions
 910     nbrows = GET_BDA(video_rows) + 1;
 911     nbcols = GET_BDA(video_cols);
 912
 913     if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
 914         // Compute the address
 915         u8 *address = (void*)(SCREEN_MEM_START(nbcols, nbrows, page)
 916                               + (xcurs + ycurs * nbcols) * 2);
 917         while (count-- > 0) {
 918             SET_FARVAR(GET_GLOBAL(vga_modes[line].sstart), *address, car);
 919             address += 2;
 920         }
 921     } else {
 922         // FIXME gfx mode not complete
 923         cheight = GET_GLOBAL(video_param_table[GET_GLOBAL(line_to_vpti[line])].cheight);
 924         bpp = GET_GLOBAL(vga_modes[line].pixbits);
 925         while ((count-- > 0) && (xcurs < nbcols)) {
 926             switch (GET_GLOBAL(vga_modes[line].memmodel)) {
 927             case PLANAR4:
 928             case PLANAR1:
 929                 write_gfx_char_pl4(car, attr, xcurs, ycurs, nbcols,
 930                                    cheight);
 931                 break;
 932             case CGA:
 933                 write_gfx_char_cga(car, attr, xcurs, ycurs, nbcols, bpp);
 934                 break;
 935             case LINEAR8:
 936                 write_gfx_char_lin(car, attr, xcurs, ycurs, nbcols);
 937                 break;
 938 #ifdef DEBUG
 939             default:
 940                 unimplemented();
 941 #endif
 942             }
 943             xcurs++;
 944         }
 945     }
 946 }
 947
 948
 949
 950 // -------------------------------------------------------------------
 951 static void
 952 biosfn_set_border_color(struct bregs *regs)
 953 {
 954     inb(VGAREG_ACTL_RESET);
 955     outb(0x00, VGAREG_ACTL_ADDRESS);
 956     u8 al = regs->bl & 0x0f;
 957     if (al & 0x08)
 958         al += 0x08;
 959     outb(al, VGAREG_ACTL_WRITE_DATA);
 960     u8 bl = regs->bl & 0x10;
 961
 962     int i;
 963     for (i = 1; i < 4; i++) {
 964         outb(i, VGAREG_ACTL_ADDRESS);
 965
 966         al = inb(VGAREG_ACTL_READ_DATA);
 967         al &= 0xef;
 968         al |= bl;
 969         outb(al, VGAREG_ACTL_WRITE_DATA);
 970     }
 971     outb(0x20, VGAREG_ACTL_ADDRESS);
 972 }
 973
 974 static void
 975 biosfn_set_palette(struct bregs *regs)
 976 {
 977     inb(VGAREG_ACTL_RESET);
 978     u8 bl = regs->bl & 0x01;
 979     int i;
 980     for (i = 1; i < 4; i++) {
 981         outb(i, VGAREG_ACTL_ADDRESS);
 982
 983         u8 al = inb(VGAREG_ACTL_READ_DATA);
 984         al &= 0xfe;
 985         al |= bl;
 986         outb(al, VGAREG_ACTL_WRITE_DATA);
 987     }
 988     outb(0x20, VGAREG_ACTL_ADDRESS);
 989 }
 990
 991 // -------------------------------------------------------------------
 992 static void
 993 biosfn_write_pixel(u8 BH, u8 AL, u16 CX, u16 DX)
 994 {
 995     u8 mask, attr, data;
 996
 997     // Get the mode
 998     u8 mode = GET_BDA(video_mode);
 999     u8 line = find_vga_entry(mode);
1000     if (line == 0xFF)
1001         return;
1002     if (GET_GLOBAL(vga_modes[line].class) == TEXT)
1003         return;
1004
1005     u8 *addr;
1006     switch (GET_GLOBAL(vga_modes[line].memmodel)) {
1007     case PLANAR4:
1008     case PLANAR1:
1009         addr = (void*)(CX / 8 + DX * GET_BDA(video_cols));
1010         mask = 0x80 >> (CX & 0x07);
1011         outw((mask << 8) | 0x08, VGAREG_GRDC_ADDRESS);
1012         outw(0x0205, VGAREG_GRDC_ADDRESS);
1013         data = GET_FARVAR(0xa000, *addr);
1014         if (AL & 0x80)
1015             outw(0x1803, VGAREG_GRDC_ADDRESS);
1016         SET_FARVAR(0xa000, *addr, AL);
1017         outw(0xff08, VGAREG_GRDC_ADDRESS);
1018         outw(0x0005, VGAREG_GRDC_ADDRESS);
1019         outw(0x0003, VGAREG_GRDC_ADDRESS);
1020         break;
1021     case CGA:
1022         if (GET_GLOBAL(vga_modes[line].pixbits) == 2)
1023             addr = (void*)((CX >> 2) + (DX >> 1) * 80);
1024         else
1025             addr = (void*)((CX >> 3) + (DX >> 1) * 80);
1026         if (DX & 1)
1027             addr += 0x2000;
1028         data = GET_FARVAR(0xb800, *addr);
1029         if (GET_GLOBAL(vga_modes[line].pixbits) == 2) {
1030             attr = (AL & 0x03) << ((3 - (CX & 0x03)) * 2);
1031             mask = 0x03 << ((3 - (CX & 0x03)) * 2);
1032         } else {
1033             attr = (AL & 0x01) << (7 - (CX & 0x07));
1034             mask = 0x01 << (7 - (CX & 0x07));
1035         }
1036         if (AL & 0x80) {
1037             data ^= attr;
1038         } else {
1039             data &= ~mask;
1040             data |= attr;
1041         }
1042         SET_FARVAR(0xb800, *addr, data);
1043         break;
1044     case LINEAR8:
1045         addr = (void*)(CX + DX * (GET_BDA(video_cols) * 8));
1046         SET_FARVAR(0xa000, *addr, AL);
1047         break;
1048 #ifdef DEBUG
1049     default:
1050         unimplemented();
1051 #endif
1052     }
1053 }
1054
1055 // -------------------------------------------------------------------
1056 static void
1057 biosfn_read_pixel(u8 BH, u16 CX, u16 DX, u16 *AX)
1058 {
1059     u8 mode, line, mask, attr, data, i;
1060
1061     // Get the mode
1062     mode = GET_BDA(video_mode);
1063     line = find_vga_entry(mode);
1064     if (line == 0xFF)
1065         return;
1066     if (GET_GLOBAL(vga_modes[line].class) == TEXT)
1067         return;
1068
1069     u8 *addr;
1070     switch (GET_GLOBAL(vga_modes[line].memmodel)) {
1071     case PLANAR4:
1072     case PLANAR1:
1073         addr = (void*)(CX / 8 + DX * GET_BDA(video_cols));
1074         mask = 0x80 >> (CX & 0x07);
1075         attr = 0x00;
1076         for (i = 0; i < 4; i++) {
1077             outw((i << 8) | 0x04, VGAREG_GRDC_ADDRESS);
1078             data = GET_FARVAR(0xa000, *addr) & mask;
1079             if (data > 0)
1080                 attr |= (0x01 << i);
1081         }
1082         break;
1083     case CGA:
1084         addr = (void*)((CX >> 2) + (DX >> 1) * 80);
1085         if (DX & 1)
1086             addr += 0x2000;
1087         data = GET_FARVAR(0xb800, *addr);
1088         if (GET_GLOBAL(vga_modes[line].pixbits) == 2)
1089             attr = (data >> ((3 - (CX & 0x03)) * 2)) & 0x03;
1090         else
1091             attr = (data >> (7 - (CX & 0x07))) & 0x01;
1092         break;
1093     case LINEAR8:
1094         addr = (void*)(CX + DX * (GET_BDA(video_cols) * 8));
1095         attr = GET_FARVAR(0xa000, *addr);
1096         break;
1097     default:
1098 #ifdef DEBUG
1099         unimplemented();
1100 #endif
1101         attr = 0;
1102     }
1103     *AX = (*AX & 0xff00) | attr;
1104 }
1105
1106 // -------------------------------------------------------------------
1107 static void
1108 biosfn_write_teletype(u8 car, u8 page, u8 attr, u8 flag)
1109 {                               // flag = WITH_ATTR / NO_ATTR
1110     u8 cheight, xcurs, ycurs, mode, line, bpp;
1111     u16 nbcols, nbrows;
1112     u16 cursor, dummy;
1113
1114     // special case if page is 0xff, use current page
1115     if (page == 0xff)
1116         page = GET_BDA(video_page);
1117
1118     // Get the mode
1119     mode = GET_BDA(video_mode);
1120     line = find_vga_entry(mode);
1121     if (line == 0xFF)
1122         return;
1123
1124     // Get the cursor pos for the page
1125     biosfn_get_cursor_pos(page, &dummy, &cursor);
1126     xcurs = cursor & 0x00ff;
1127     ycurs = (cursor & 0xff00) >> 8;
1128
1129     // Get the dimensions
1130     nbrows = GET_BDA(video_rows) + 1;
1131     nbcols = GET_BDA(video_cols);
1132
1133     switch (car) {
1134     case 7:
1135         //FIXME should beep
1136         break;
1137
1138     case 8:
1139         if (xcurs > 0)
1140             xcurs--;
1141         break;
1142
1143     case '\r':
1144         xcurs = 0;
1145         break;
1146
1147     case '\n':
1148         ycurs++;
1149         break;
1150
1151     case '\t':
1152         do {
1153             biosfn_write_teletype(' ', page, attr, flag);
1154             biosfn_get_cursor_pos(page, &dummy, &cursor);
1155             xcurs = cursor & 0x00ff;
1156             ycurs = (cursor & 0xff00) >> 8;
1157         } while (xcurs % 8 == 0);
1158         break;
1159
1160     default:
1161
1162         if (GET_GLOBAL(vga_modes[line].class) == TEXT) {
1163             // Compute the address
1164             u8 *address = (void*)(SCREEN_MEM_START(nbcols, nbrows, page)
1165                                   + (xcurs + ycurs * nbcols) * 2);
1166             // Write the char
1167             SET_FARVAR(GET_GLOBAL(vga_modes[line].sstart), address[0], car);
1168             if (flag == WITH_ATTR)
1169                 SET_FARVAR(GET_GLOBAL(vga_modes[line].sstart), address[1], attr);
1170         } else {
1171             // FIXME gfx mode not complete
1172             cheight = GET_GLOBAL(video_param_table[GET_GLOBAL(line_to_vpti[line])].cheight);
1173             bpp = GET_GLOBAL(vga_modes[line].pixbits);
1174             switch (GET_GLOBAL(vga_modes[line].memmodel)) {
1175             case PLANAR4:
1176             case PLANAR1:
1177                 write_gfx_char_pl4(car, attr, xcurs, ycurs, nbcols,
1178                                    cheight);
1179                 break;
1180             case CGA:
1181                 write_gfx_char_cga(car, attr, xcurs, ycurs, nbcols, bpp);
1182                 break;
1183             case LINEAR8:
1184                 write_gfx_char_lin(car, attr, xcurs, ycurs, nbcols);
1185                 break;
1186 #ifdef DEBUG
1187             default:
1188                 unimplemented();
1189 #endif
1190             }
1191         }
1192         xcurs++;
1193     }
1194
1195     // Do we need to wrap ?
1196     if (xcurs == nbcols) {
1197         xcurs = 0;
1198         ycurs++;
1199     }
1200     // Do we need to scroll ?
1201     if (ycurs == nbrows) {
1202         if (GET_GLOBAL(vga_modes[line].class) == TEXT)
1203             biosfn_scroll(0x01, 0x07, 0, 0, nbrows - 1, nbcols - 1, page,
1204                           SCROLL_UP);
1205         else
1206             biosfn_scroll(0x01, 0x00, 0, 0, nbrows - 1, nbcols - 1, page,
1207                           SCROLL_UP);
1208         ycurs -= 1;
1209     }
1210     // Set the cursor for the page
1211     cursor = ycurs;
1212     cursor <<= 8;
1213     cursor += xcurs;
1214     biosfn_set_cursor_pos(page, cursor);
1215 }
1216
1217 // -------------------------------------------------------------------
1218 static void
1219 biosfn_get_video_mode(struct bregs *regs)
1220 {
1221     regs->bh = GET_BDA(video_page);
1222     regs->al = GET_BDA(video_mode) | (GET_BDA(video_ctl) & 0x80);
1223     regs->ah = GET_BDA(video_cols);
1224 }
1225
1226 // -------------------------------------------------------------------
1227 static void
1228 biosfn_set_overscan_border_color(struct bregs *regs)
1229 {
1230     inb(VGAREG_ACTL_RESET);
1231     outb(0x11, VGAREG_ACTL_ADDRESS);
1232     outb(regs->bh, VGAREG_ACTL_WRITE_DATA);
1233     outb(0x20, VGAREG_ACTL_ADDRESS);
1234 }
1235
1236 // -------------------------------------------------------------------
1237 static void
1238 biosfn_set_all_palette_reg(struct bregs *regs)
1239 {
1240     inb(VGAREG_ACTL_RESET);
1241
1242     u8 *data = (u8*)(regs->dx + 0);
1243     int i;
1244     for (i = 0; i < 0x10; i++) {
1245         outb(i, VGAREG_ACTL_ADDRESS);
1246         u8 val = GET_FARVAR(regs->es, *data);
1247         outb(val, VGAREG_ACTL_WRITE_DATA);
1248         data++;
1249     }
1250     outb(0x11, VGAREG_ACTL_ADDRESS);
1251     outb(GET_FARVAR(regs->es, *data), VGAREG_ACTL_WRITE_DATA);
1252     outb(0x20, VGAREG_ACTL_ADDRESS);
1253 }
1254
1255 // -------------------------------------------------------------------
1256 static void
1257 biosfn_toggle_intensity(struct bregs *regs)
1258 {
1259     inb(VGAREG_ACTL_RESET);
1260     outb(0x10, VGAREG_ACTL_ADDRESS);
1261     u8 val = (inb(VGAREG_ACTL_READ_DATA) & 0x7f) | ((regs->bl & 0x01) << 3);
1262     outb(val, VGAREG_ACTL_WRITE_DATA);
1263     outb(0x20, VGAREG_ACTL_ADDRESS);
1264 }
1265
1266 // -------------------------------------------------------------------
1267 void
1268 biosfn_set_single_palette_reg(u8 reg, u8 val)
1269 {
1270     inb(VGAREG_ACTL_RESET);
1271     outb(reg, VGAREG_ACTL_ADDRESS);
1272     outb(val, VGAREG_ACTL_WRITE_DATA);
1273     outb(0x20, VGAREG_ACTL_ADDRESS);
1274 }
1275
1276 // -------------------------------------------------------------------
1277 u8
1278 biosfn_get_single_palette_reg(u8 reg)
1279 {
1280     inb(VGAREG_ACTL_RESET);
1281     outb(reg, VGAREG_ACTL_ADDRESS);
1282     u8 v = inb(VGAREG_ACTL_READ_DATA);
1283     inb(VGAREG_ACTL_RESET);
1284     outb(0x20, VGAREG_ACTL_ADDRESS);
1285     return v;
1286 }
1287
1288 // -------------------------------------------------------------------
1289 static void
1290 biosfn_read_overscan_border_color(struct bregs *regs)
1291 {
1292     inb(VGAREG_ACTL_RESET);
1293     outb(0x11, VGAREG_ACTL_ADDRESS);
1294     regs->bh = inb(VGAREG_ACTL_READ_DATA);
1295     inb(VGAREG_ACTL_RESET);
1296     outb(0x20, VGAREG_ACTL_ADDRESS);
1297 }
1298
1299 // -------------------------------------------------------------------
1300 static void
1301 biosfn_get_all_palette_reg(struct bregs *regs)
1302 {
1303     u8 *data = (u8*)(regs->dx + 0);
1304     int i;
1305     for (i = 0; i < 0x10; i++) {
1306         inb(VGAREG_ACTL_RESET);
1307         outb(i, VGAREG_ACTL_ADDRESS);
1308         SET_FARVAR(regs->es, *data, inb(VGAREG_ACTL_READ_DATA));
1309         data++;
1310     }
1311     inb(VGAREG_ACTL_RESET);
1312     outb(0x11, VGAREG_ACTL_ADDRESS);
1313     SET_FARVAR(regs->es, *data, inb(VGAREG_ACTL_READ_DATA));
1314     inb(VGAREG_ACTL_RESET);
1315     outb(0x20, VGAREG_ACTL_ADDRESS);
1316 }
1317
1318 // -------------------------------------------------------------------
1319 static void
1320 biosfn_set_single_dac_reg(struct bregs *regs)
1321 {
1322     outb(regs->bl, VGAREG_DAC_WRITE_ADDRESS);
1323     outb(regs->dh, VGAREG_DAC_DATA);
1324     outb(regs->ch, VGAREG_DAC_DATA);
1325     outb(regs->cl, VGAREG_DAC_DATA);
1326 }
1327
1328 // -------------------------------------------------------------------
1329 static void
1330 biosfn_set_all_dac_reg(struct bregs *regs)
1331 {
1332     outb(regs->bl, VGAREG_DAC_WRITE_ADDRESS);
1333     u8 *data = (u8*)(regs->dx + 0);
1334     int count = regs->cx;
1335     while (count) {
1336         outb(GET_FARVAR(regs->es, *data), VGAREG_DAC_DATA);
1337         data++;
1338         outb(GET_FARVAR(regs->es, *data), VGAREG_DAC_DATA);
1339         data++;
1340         outb(GET_FARVAR(regs->es, *data), VGAREG_DAC_DATA);
1341         data++;
1342         count--;
1343     }
1344 }
1345
1346 // -------------------------------------------------------------------
1347 static void
1348 biosfn_select_video_dac_color_page(struct bregs *regs)
1349 {
1350     inb(VGAREG_ACTL_RESET);
1351     outb(0x10, VGAREG_ACTL_ADDRESS);
1352     u8 val = inb(VGAREG_ACTL_READ_DATA);
1353     if (!(regs->bl & 0x01)) {
1354         val = (val & 0x7f) | (regs->bh << 7);
1355         outb(val, VGAREG_ACTL_WRITE_DATA);
1356         outb(0x20, VGAREG_ACTL_ADDRESS);
1357         return;
1358     }
1359     inb(VGAREG_ACTL_RESET);
1360     outb(0x14, VGAREG_ACTL_ADDRESS);
1361     u8 bh = regs->bh;
1362     if (!(val & 0x80))
1363         bh <<= 2;
1364     bh &= 0x0f;
1365     outb(bh, VGAREG_ACTL_WRITE_DATA);
1366     outb(0x20, VGAREG_ACTL_ADDRESS);
1367 }
1368
1369 // -------------------------------------------------------------------
1370 static void
1371 biosfn_read_single_dac_reg(struct bregs *regs)
1372 {
1373     outb(regs->bl, VGAREG_DAC_READ_ADDRESS);
1374     regs->dh = inb(VGAREG_DAC_DATA);
1375     regs->ch = inb(VGAREG_DAC_DATA);
1376     regs->cl = inb(VGAREG_DAC_DATA);
1377 }
1378
1379 // -------------------------------------------------------------------
1380 static void
1381 biosfn_read_all_dac_reg(struct bregs *regs)
1382 {
1383     outb(regs->bl, VGAREG_DAC_READ_ADDRESS);
1384     u8 *data = (u8*)(regs->dx + 0);
1385     int count = regs->cx;
1386     while (count) {
1387         SET_FARVAR(regs->es, *data, inb(VGAREG_DAC_DATA));
1388         data++;
1389         SET_FARVAR(regs->es, *data, inb(VGAREG_DAC_DATA));
1390         data++;
1391         SET_FARVAR(regs->es, *data, inb(VGAREG_DAC_DATA));
1392         data++;
1393         count--;
1394     }
1395 }
1396
1397 // -------------------------------------------------------------------
1398 static void
1399 biosfn_set_pel_mask(struct bregs *regs)
1400 {
1401     outb(regs->bl, VGAREG_PEL_MASK);
1402 }
1403
1404 // -------------------------------------------------------------------
1405 static void
1406 biosfn_read_pel_mask(struct bregs *regs)
1407 {
1408     regs->bl = inb(VGAREG_PEL_MASK);
1409 }
1410
1411 // -------------------------------------------------------------------
1412 static void
1413 biosfn_read_video_dac_state(struct bregs *regs)
1414 {
1415     inb(VGAREG_ACTL_RESET);
1416     outb(0x10, VGAREG_ACTL_ADDRESS);
1417     u8 val1 = inb(VGAREG_ACTL_READ_DATA) >> 7;
1418
1419     inb(VGAREG_ACTL_RESET);
1420     outb(0x14, VGAREG_ACTL_ADDRESS);
1421     u8 val2 = inb(VGAREG_ACTL_READ_DATA) & 0x0f;
1422     if (!(val1 & 0x01))
1423         val2 >>= 2;
1424
1425     inb(VGAREG_ACTL_RESET);
1426     outb(0x20, VGAREG_ACTL_ADDRESS);
1427
1428     regs->bl = val1;
1429     regs->bh = val2;
1430 }
1431
1432 // -------------------------------------------------------------------
1433 static void
1434 get_font_access()
1435 {
1436     outw(0x0100, VGAREG_SEQU_ADDRESS);
1437     outw(0x0402, VGAREG_SEQU_ADDRESS);
1438     outw(0x0704, VGAREG_SEQU_ADDRESS);
1439     outw(0x0300, VGAREG_SEQU_ADDRESS);
1440     outw(0x0204, VGAREG_GRDC_ADDRESS);
1441     outw(0x0005, VGAREG_GRDC_ADDRESS);
1442     outw(0x0406, VGAREG_GRDC_ADDRESS);
1443 }
1444
1445 static void
1446 release_font_access()
1447 {
1448     outw(0x0100, VGAREG_SEQU_ADDRESS);
1449     outw(0x0302, VGAREG_SEQU_ADDRESS);
1450     outw(0x0304, VGAREG_SEQU_ADDRESS);
1451     outw(0x0300, VGAREG_SEQU_ADDRESS);
1452     u16 v = inw(VGAREG_READ_MISC_OUTPUT);
1453     v = ((v & 0x01) << 10) | 0x0a06;
1454     outw(v, VGAREG_GRDC_ADDRESS);
1455     outw(0x0004, VGAREG_GRDC_ADDRESS);
1456     outw(0x1005, VGAREG_GRDC_ADDRESS);
1457 }
1458
1459 static void
1460 set_scan_lines(u8 lines)
1461 {
1462     u16 crtc_addr, cols, vde;
1463     u8 crtc_r9, ovl, rows;
1464
1465     crtc_addr = GET_BDA(crtc_address);
1466     outb(0x09, crtc_addr);
1467     crtc_r9 = inb(crtc_addr + 1);
1468     crtc_r9 = (crtc_r9 & 0xe0) | (lines - 1);
1469     outb(crtc_r9, crtc_addr + 1);
1470     if (lines == 8)
1471         biosfn_set_cursor_shape(0x06, 0x07);
1472     else
1473         biosfn_set_cursor_shape(lines - 4, lines - 3);
1474     SET_BDA(char_height, lines);
1475     outb(0x12, crtc_addr);
1476     vde = inb(crtc_addr + 1);
1477     outb(0x07, crtc_addr);
1478     ovl = inb(crtc_addr + 1);
1479     vde += (((ovl & 0x02) << 7) + ((ovl & 0x40) << 3) + 1);
1480     rows = vde / lines;
1481     SET_BDA(video_rows, rows - 1);
1482     cols = GET_BDA(video_cols);
1483     SET_BDA(video_pagesize, rows * cols * 2);
1484 }
1485
1486 static void
1487 biosfn_load_text_user_pat(u8 AL, u16 ES, u16 BP, u16 CX, u16 DX, u8 BL,
1488                           u8 BH)
1489 {
1490     get_font_access();
1491     u16 blockaddr = ((BL & 0x03) << 14) + ((BL & 0x04) << 11);
1492     u16 i;
1493     for (i = 0; i < CX; i++) {
1494         void *src = (void*)(BP + i * BH);
1495         void *dest = (void*)(blockaddr + (DX + i) * 32);
1496         memcpy_far(0xA000, dest, ES, src, BH);
1497     }
1498     release_font_access();
1499     if (AL >= 0x10)
1500         set_scan_lines(BH);
1501 }
1502
1503 static void
1504 biosfn_load_text_8_14_pat(u8 AL, u8 BL)
1505 {
1506     get_font_access();
1507     u16 blockaddr = ((BL & 0x03) << 14) + ((BL & 0x04) << 11);
1508     u16 i;
1509     for (i = 0; i < 0x100; i++) {
1510         u16 src = i * 14;
1511         void *dest = (void*)(blockaddr + i * 32);
1512         memcpy_far(0xA000, dest, 0xC000, &vgafont14[src], 14);
1513     }
1514     release_font_access();
1515     if (AL >= 0x10)
1516         set_scan_lines(14);
1517 }
1518
1519 static void
1520 biosfn_load_text_8_8_pat(u8 AL, u8 BL)
1521 {
1522     get_font_access();
1523     u16 blockaddr = ((BL & 0x03) << 14) + ((BL & 0x04) << 11);
1524     u16 i;
1525     for (i = 0; i < 0x100; i++) {
1526         u16 src = i * 8;
1527         void *dest = (void*)(blockaddr + i * 32);
1528         memcpy_far(0xA000, dest, 0xC000, &vgafont8[src], 8);
1529     }
1530     release_font_access();
1531     if (AL >= 0x10)
1532         set_scan_lines(8);
1533 }
1534
1535 // -------------------------------------------------------------------
1536 static void
1537 biosfn_set_text_block_specifier(struct bregs *regs)
1538 {
1539     outw((regs->bl << 8) | 0x03, VGAREG_SEQU_ADDRESS);
1540 }
1541
1542 // -------------------------------------------------------------------
1543 static void
1544 biosfn_load_text_8_16_pat(u8 AL, u8 BL)
1545 {
1546     get_font_access();
1547     u16 blockaddr = ((BL & 0x03) << 14) + ((BL & 0x04) << 11);
1548     u16 i;
1549     for (i = 0; i < 0x100; i++) {
1550         u16 src = i * 16;
1551         void *dest = (void*)(blockaddr + i * 32);
1552         memcpy_far(0xA000, dest, 0xC000, &vgafont16[src], 16);
1553     }
1554     release_font_access();
1555     if (AL >= 0x10)
1556         set_scan_lines(16);
1557 }
1558
1559 // -------------------------------------------------------------------
1560 static void
1561 biosfn_get_font_info(u8 BH, u16 *ES, u16 *BP, u16 *CX, u16 *DX)
1562 {
1563     switch (BH) {
1564     case 0x00: {
1565         u32 segoff = GET_IVT(0x1f).segoff;
1566         *ES = segoff >> 16;
1567         *BP = segoff;
1568         break;
1569     }
1570     case 0x01: {
1571         u32 segoff = GET_IVT(0x43).segoff;
1572         *ES = segoff >> 16;
1573         *BP = segoff;
1574         break;
1575     }
1576     case 0x02:
1577         *ES = 0xC000;
1578         *BP = (u32)vgafont14;
1579         break;
1580     case 0x03:
1581         *ES = 0xC000;
1582         *BP = (u32)vgafont8;
1583         break;
1584     case 0x04:
1585         *ES = 0xC000;
1586         *BP = (u32)vgafont8 + 128 * 8;
1587         break;
1588     case 0x05:
1589         *ES = 0xC000;
1590         *BP = (u32)vgafont14alt;
1591         break;
1592     case 0x06:
1593         *ES = 0xC000;
1594         *BP = (u32)vgafont16;
1595         break;
1596     case 0x07:
1597         *ES = 0xC000;
1598         *BP = (u32)vgafont16alt;
1599         break;
1600     default:
1601         dprintf(1, "Get font info BH(%02x) was discarded\n", BH);
1602         return;
1603     }
1604     // Set byte/char of on screen font
1605     *CX = GET_BDA(char_height) & 0xff;
1606
1607     // Set Highest char row
1608     *DX = GET_BDA(video_rows);
1609 }
1610
1611 // -------------------------------------------------------------------
1612 static void
1613 biosfn_get_ega_info(struct bregs *regs)
1614 {
1615     regs->cx = GET_BDA(video_switches) & 0x0f;
1616     regs->ax = GET_BDA(crtc_address);
1617     if (regs->ax == VGAREG_MDA_CRTC_ADDRESS)
1618         regs->bx = 0x0103;
1619     else
1620         regs->bx = 0x0003;
1621 }
1622
1623 // -------------------------------------------------------------------
1624 static void
1625 biosfn_select_vert_res(struct bregs *regs)
1626 {
1627     u8 mctl = GET_BDA(modeset_ctl);
1628     u8 vswt = GET_BDA(video_switches);
1629
1630     switch (regs->al) {
1631     case 0x00:
1632         // 200 lines
1633         mctl = (mctl & ~0x10) | 0x80;
1634         vswt = (vswt & ~0x0f) | 0x08;
1635         break;
1636     case 0x01:
1637         // 350 lines
1638         mctl &= ~0x90;
1639         vswt = (vswt & ~0x0f) | 0x09;
1640         break;
1641     case 0x02:
1642         // 400 lines
1643         mctl = (mctl & ~0x80) | 0x10;
1644         vswt = (vswt & ~0x0f) | 0x09;
1645         break;
1646     default:
1647         dprintf(1, "Select vert res (%02x) was discarded\n", regs->al);
1648         break;
1649     }
1650     SET_BDA(modeset_ctl, mctl);
1651     SET_BDA(video_switches, vswt);
1652     regs->ax = 0x1212;
1653 }
1654
1655 static void
1656 biosfn_enable_default_palette_loading(struct bregs *regs)
1657 {
1658     u8 v = (regs->al & 0x01) << 3;
1659     u8 mctl = GET_BDA(video_ctl) & ~0x08;
1660     SET_BDA(video_ctl, mctl | v);
1661     regs->ax = 0x1212;
1662 }
1663
1664 static void
1665 biosfn_enable_video_addressing(struct bregs *regs)
1666 {
1667     u8 v = ((regs->al << 1) & 0x02) ^ 0x02;
1668     u8 v2 = inb(VGAREG_READ_MISC_OUTPUT) & ~0x02;
1669     outb(v | v2, VGAREG_WRITE_MISC_OUTPUT);
1670     regs->ax = 0x1212;
1671 }
1672
1673
1674 static void
1675 biosfn_enable_grayscale_summing(struct bregs *regs)
1676 {
1677     u8 v = ((regs->al << 1) & 0x02) ^ 0x02;
1678     u8 v2 = GET_BDA(modeset_ctl) & ~0x02;
1679     SET_BDA(modeset_ctl, v | v2);
1680     regs->ax = 0x1212;
1681 }
1682
1683 static void
1684 biosfn_enable_cursor_emulation(struct bregs *regs)
1685 {
1686     u8 v = (regs->al & 0x01) ^ 0x01;
1687     u8 v2 = GET_BDA(modeset_ctl) & ~0x01;
1688     SET_BDA(modeset_ctl, v | v2);
1689     regs->ax = 0x1212;
1690 }
1691
1692 // -------------------------------------------------------------------
1693 static void
1694 biosfn_write_string(u8 flag, u8 page, u8 attr, u16 count, u8 row, u8 col,
1695                     u16 seg, u8 *offset)
1696 {
1697     u16 newcurs, oldcurs, dummy;
1698     u8 car;
1699
1700     // Read curs info for the page
1701     biosfn_get_cursor_pos(page, &dummy, &oldcurs);
1702
1703     // if row=0xff special case : use current cursor position
1704     if (row == 0xff) {
1705         col = oldcurs & 0x00ff;
1706         row = (oldcurs & 0xff00) >> 8;
1707     }
1708
1709     newcurs = row;
1710     newcurs <<= 8;
1711     newcurs += col;
1712     biosfn_set_cursor_pos(page, newcurs);
1713
1714     while (count-- != 0) {
1715         car = GET_FARVAR(seg, *offset);
1716         offset++;
1717         if ((flag & 0x02) != 0) {
1718             attr = GET_FARVAR(seg, *offset);
1719             offset++;
1720         }
1721
1722         biosfn_write_teletype(car, page, attr, WITH_ATTR);
1723     }
1724
1725     // Set back curs pos
1726     if ((flag & 0x01) == 0)
1727         biosfn_set_cursor_pos(page, oldcurs);
1728 }
1729
1730 // -------------------------------------------------------------------
1731 static void
1732 biosfn_read_display_code(struct bregs *regs)
1733 {
1734     regs->bx = GET_BDA(dcc_index);
1735     regs->al = 0x1a;
1736 }
1737
1738 static void
1739 biosfn_set_display_code(struct bregs *regs)
1740 {
1741     SET_BDA(dcc_index, regs->bl);
1742     dprintf(1, "Alternate Display code (%02x) was discarded\n", regs->bh);
1743     regs->al = 0x1a;
1744 }
1745
1746 // -------------------------------------------------------------------
1747 static void
1748 biosfn_read_state_info(u16 BX, u16 ES, u16 DI)
1749 {
1750     // Address of static functionality table
1751     SET_FARVAR(ES, *(u16*)(DI + 0x00), (u32)static_functionality);
1752     SET_FARVAR(ES, *(u16*)(DI + 0x02), 0xC000);
1753
1754     // Hard coded copy from BIOS area. Should it be cleaner ?
1755     memcpy_far(ES, (void*)(DI + 0x04), SEG_BDA, (void*)0x49, 30);
1756     memcpy_far(ES, (void*)(DI + 0x22), SEG_BDA, (void*)0x84, 3);
1757
1758     SET_FARVAR(ES, *(u8*)(DI + 0x25), GET_BDA(dcc_index));
1759     SET_FARVAR(ES, *(u8*)(DI + 0x26), 0);
1760     SET_FARVAR(ES, *(u8*)(DI + 0x27), 16);
1761     SET_FARVAR(ES, *(u8*)(DI + 0x28), 0);
1762     SET_FARVAR(ES, *(u8*)(DI + 0x29), 8);
1763     SET_FARVAR(ES, *(u8*)(DI + 0x2a), 2);
1764     SET_FARVAR(ES, *(u8*)(DI + 0x2b), 0);
1765     SET_FARVAR(ES, *(u8*)(DI + 0x2c), 0);
1766     SET_FARVAR(ES, *(u8*)(DI + 0x31), 3);
1767     SET_FARVAR(ES, *(u8*)(DI + 0x32), 0);
1768
1769     memset_far(ES, (void*)(DI + 0x33), 0, 13);
1770 }
1771
1772 // -------------------------------------------------------------------
1773 // -------------------------------------------------------------------
1774 static u16
1775 biosfn_read_video_state_size(u16 CX)
1776 {
1777     u16 size = 0;
1778     if (CX & 1)
1779         size += 0x46;
1780     if (CX & 2)
1781         size += (5 + 8 + 5) * 2 + 6;
1782     if (CX & 4)
1783         size += 3 + 256 * 3 + 1;
1784     return size;
1785 }
1786
1787 static u16
1788 biosfn_save_video_state(u16 CX, u16 ES, u16 BX)
1789 {
1790     u16 i, crtc_addr, ar_index;
1791
1792     crtc_addr = GET_BDA(crtc_address);
1793     if (CX & 1) {
1794         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_ADDRESS));
1795         BX++;
1796         SET_FARVAR(ES, *(u8*)(BX+0), inb(crtc_addr));
1797         BX++;
1798         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_GRDC_ADDRESS));
1799         BX++;
1800         inb(VGAREG_ACTL_RESET);
1801         ar_index = inb(VGAREG_ACTL_ADDRESS);
1802         SET_FARVAR(ES, *(u8*)(BX+0), ar_index);
1803         BX++;
1804         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_READ_FEATURE_CTL));
1805         BX++;
1806
1807         for (i = 1; i <= 4; i++) {
1808             outb(i, VGAREG_SEQU_ADDRESS);
1809             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_DATA));
1810             BX++;
1811         }
1812         outb(0, VGAREG_SEQU_ADDRESS);
1813         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_SEQU_DATA));
1814         BX++;
1815
1816         for (i = 0; i <= 0x18; i++) {
1817             outb(i, crtc_addr);
1818             SET_FARVAR(ES, *(u8*)(BX+0), inb(crtc_addr + 1));
1819             BX++;
1820         }
1821
1822         for (i = 0; i <= 0x13; i++) {
1823             inb(VGAREG_ACTL_RESET);
1824             outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
1825             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_ACTL_READ_DATA));
1826             BX++;
1827         }
1828         inb(VGAREG_ACTL_RESET);
1829
1830         for (i = 0; i <= 8; i++) {
1831             outb(i, VGAREG_GRDC_ADDRESS);
1832             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_GRDC_DATA));
1833             BX++;
1834         }
1835
1836         SET_FARVAR(ES, *(u16*)(BX+0), crtc_addr);
1837         BX += 2;
1838
1839         /* XXX: read plane latches */
1840         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1841         BX++;
1842         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1843         BX++;
1844         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1845         BX++;
1846         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1847         BX++;
1848     }
1849     if (CX & 2) {
1850         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_mode));
1851         BX++;
1852         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_cols));
1853         BX += 2;
1854         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_pagesize));
1855         BX += 2;
1856         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(crtc_address));
1857         BX += 2;
1858         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_rows));
1859         BX++;
1860         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(char_height));
1861         BX += 2;
1862         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_ctl));
1863         BX++;
1864         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_switches));
1865         BX++;
1866         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(modeset_ctl));
1867         BX++;
1868         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(cursor_type));
1869         BX += 2;
1870         for (i = 0; i < 8; i++) {
1871             SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(cursor_pos[i]));
1872             BX += 2;
1873         }
1874         SET_FARVAR(ES, *(u16*)(BX+0), GET_BDA(video_pagestart));
1875         BX += 2;
1876         SET_FARVAR(ES, *(u8*)(BX+0), GET_BDA(video_page));
1877         BX++;
1878         /* current font */
1879         SET_FARVAR(ES, *(u16*)(BX+0), GET_FARVAR(0, *(u16*)(0x1f * 4)));
1880         BX += 2;
1881         SET_FARVAR(ES, *(u16*)(BX+0), GET_FARVAR(0, *(u16*)(0x1f * 4 + 2)));
1882         BX += 2;
1883         SET_FARVAR(ES, *(u16*)(BX+0), GET_FARVAR(0, *(u16*)(0x43 * 4)));
1884         BX += 2;
1885         SET_FARVAR(ES, *(u16*)(BX+0), GET_FARVAR(0, *(u16*)(0x43 * 4 + 2)));
1886         BX += 2;
1887     }
1888     if (CX & 4) {
1889         /* XXX: check this */
1890         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_STATE));
1891         BX++;                   /* read/write mode dac */
1892         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_WRITE_ADDRESS));
1893         BX++;                   /* pix address */
1894         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_PEL_MASK));
1895         BX++;
1896         // Set the whole dac always, from 0
1897         outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
1898         for (i = 0; i < 256 * 3; i++) {
1899             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_DATA));
1900             BX++;
1901         }
1902         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1903         BX++;                   /* color select register */
1904     }
1905     return BX;
1906 }
1907
1908 static u16
1909 biosfn_restore_video_state(u16 CX, u16 ES, u16 BX)
1910 {
1911     u16 i, crtc_addr, v, addr1, ar_index;
1912
1913     if (CX & 1) {
1914         // Reset Attribute Ctl flip-flop
1915         inb(VGAREG_ACTL_RESET);
1916
1917         crtc_addr = GET_FARVAR(ES, *(u16*)(BX + 0x40));
1918         addr1 = BX;
1919         BX += 5;
1920
1921         for (i = 1; i <= 4; i++) {
1922             outb(i, VGAREG_SEQU_ADDRESS);
1923             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_SEQU_DATA);
1924             BX++;
1925         }
1926         outb(0, VGAREG_SEQU_ADDRESS);
1927         outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_SEQU_DATA);
1928         BX++;
1929
1930         // Disable CRTC write protection
1931         outw(0x0011, crtc_addr);
1932         // Set CRTC regs
1933         for (i = 0; i <= 0x18; i++) {
1934             if (i != 0x11) {
1935                 outb(i, crtc_addr);
1936                 outb(GET_FARVAR(ES, *(u8*)(BX+0)), crtc_addr + 1);
1937             }
1938             BX++;
1939         }
1940         // select crtc base address
1941         v = inb(VGAREG_READ_MISC_OUTPUT) & ~0x01;
1942         if (crtc_addr == VGAREG_VGA_CRTC_ADDRESS)
1943             v |= 0x01;
1944         outb(v, VGAREG_WRITE_MISC_OUTPUT);
1945
1946         // enable write protection if needed
1947         outb(0x11, crtc_addr);
1948         outb(GET_FARVAR(ES, *(u8*)(BX - 0x18 + 0x11)), crtc_addr + 1);
1949
1950         // Set Attribute Ctl
1951         ar_index = GET_FARVAR(ES, *(u8*)(addr1 + 0x03));
1952         inb(VGAREG_ACTL_RESET);
1953         for (i = 0; i <= 0x13; i++) {
1954             outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
1955             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_ACTL_WRITE_DATA);
1956             BX++;
1957         }
1958         outb(ar_index, VGAREG_ACTL_ADDRESS);
1959         inb(VGAREG_ACTL_RESET);
1960
1961         for (i = 0; i <= 8; i++) {
1962             outb(i, VGAREG_GRDC_ADDRESS);
1963             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_GRDC_DATA);
1964             BX++;
1965         }
1966         BX += 2;                /* crtc_addr */
1967         BX += 4;                /* plane latches */
1968
1969         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), VGAREG_SEQU_ADDRESS);
1970         addr1++;
1971         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), crtc_addr);
1972         addr1++;
1973         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), VGAREG_GRDC_ADDRESS);
1974         addr1++;
1975         addr1++;
1976         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), crtc_addr - 0x4 + 0xa);
1977         addr1++;
1978     }
1979     if (CX & 2) {
1980         SET_BDA(video_mode, GET_FARVAR(ES, *(u8*)(BX+0)));
1981         BX++;
1982         SET_BDA(video_cols, GET_FARVAR(ES, *(u16*)(BX+0)));
1983         BX += 2;
1984         SET_BDA(video_pagesize, GET_FARVAR(ES, *(u16*)(BX+0)));
1985         BX += 2;
1986         SET_BDA(crtc_address, GET_FARVAR(ES, *(u16*)(BX+0)));
1987         BX += 2;
1988         SET_BDA(video_rows, GET_FARVAR(ES, *(u8*)(BX+0)));
1989         BX++;
1990         SET_BDA(char_height, GET_FARVAR(ES, *(u16*)(BX+0)));
1991         BX += 2;
1992         SET_BDA(video_ctl, GET_FARVAR(ES, *(u8*)(BX+0)));
1993         BX++;
1994         SET_BDA(video_switches, GET_FARVAR(ES, *(u8*)(BX+0)));
1995         BX++;
1996         SET_BDA(modeset_ctl, GET_FARVAR(ES, *(u8*)(BX+0)));
1997         BX++;
1998         SET_BDA(cursor_type, GET_FARVAR(ES, *(u16*)(BX+0)));
1999         BX += 2;
2000         for (i = 0; i < 8; i++) {
2001             SET_BDA(cursor_pos[i], GET_FARVAR(ES, *(u16*)(BX+0)));
2002             BX += 2;
2003         }
2004         SET_BDA(video_pagestart, GET_FARVAR(ES, *(u16*)(BX+0)));
2005         BX += 2;
2006         SET_BDA(video_page, GET_FARVAR(ES, *(u8*)(BX+0)));
2007         BX++;
2008         /* current font */
2009         SET_IVT(0x1f, GET_FARVAR(ES, *(u16*)(BX+2)), GET_FARVAR(ES, *(u16*)(BX+0)));
2010         BX += 4;
2011         SET_IVT(0x43, GET_FARVAR(ES, *(u16*)(BX+2)), GET_FARVAR(ES, *(u16*)(BX+0)));
2012         BX += 4;
2013     }
2014     if (CX & 4) {
2015         BX++;
2016         v = GET_FARVAR(ES, *(u8*)(BX+0));
2017         BX++;
2018         outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_PEL_MASK);
2019         BX++;
2020         // Set the whole dac always, from 0
2021         outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
2022         for (i = 0; i < 256 * 3; i++) {
2023             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_DAC_DATA);
2024             BX++;
2025         }
2026         BX++;
2027         outb(v, VGAREG_DAC_WRITE_ADDRESS);
2028     }
2029     return BX;
2030 }
2031
2032
2033 /****************************************************************
2034  * VGA int 10 handler
2035  ****************************************************************/
2036
2037 static void
2038 handle_1000(struct bregs *regs)
2039 {
2040     // XXX - inline
2041     biosfn_set_video_mode(regs->al);
2042     switch(regs->al & 0x7F) {
2043     case 6:
2044         regs->al = 0x3F;
2045         break;
2046     case 0:
2047     case 1:
2048     case 2:
2049     case 3:
2050     case 4:
2051     case 5:
2052     case 7:
2053         regs->al = 0x30;
2054         break;
2055     default:
2056         regs->al = 0x20;
2057     }
2058 }
2059
2060 static void
2061 handle_1001(struct bregs *regs)
2062 {
2063     biosfn_set_cursor_shape(regs->ch, regs->cl);
2064 }
2065
2066 static void
2067 handle_1002(struct bregs *regs)
2068 {
2069     biosfn_set_cursor_pos(regs->bh, regs->dx);
2070 }
2071
2072 static void
2073 handle_1003(struct bregs *regs)
2074 {
2075     biosfn_get_cursor_pos(regs->bh, &regs->cx, &regs->dx);
2076 }
2077
2078 // Read light pen pos (unimplemented)
2079 static void
2080 handle_1004(struct bregs *regs)
2081 {
2082     debug_stub(regs);
2083     regs->ax = regs->bx = regs->cx = regs->dx = 0;
2084 }
2085
2086 static void
2087 handle_1005(struct bregs *regs)
2088 {
2089     biosfn_set_active_page(regs->al);
2090 }
2091
2092 static void
2093 handle_1006(struct bregs *regs)
2094 {
2095     biosfn_scroll(regs->al, regs->bh, regs->ch, regs->cl, regs->dh, regs->dl
2096                   , 0xFF, SCROLL_UP);
2097 }
2098
2099 static void
2100 handle_1007(struct bregs *regs)
2101 {
2102     biosfn_scroll(regs->al, regs->bh, regs->ch, regs->cl, regs->dh, regs->dl
2103                   , 0xFF, SCROLL_DOWN);
2104 }
2105
2106 static void
2107 handle_1008(struct bregs *regs)
2108 {
2109     // XXX - inline
2110     biosfn_read_char_attr(regs->bh, &regs->ax);
2111 }
2112
2113 static void
2114 handle_1009(struct bregs *regs)
2115 {
2116     // XXX - inline
2117     biosfn_write_char_attr(regs->al, regs->bh, regs->bl, regs->cx);
2118 }
2119
2120 static void
2121 handle_100a(struct bregs *regs)
2122 {
2123     // XXX - inline
2124     biosfn_write_char_only(regs->al, regs->bh, regs->bl, regs->cx);
2125 }
2126
2127
2128 static void
2129 handle_100b00(struct bregs *regs)
2130 {
2131     // XXX - inline
2132     biosfn_set_border_color(regs);
2133 }
2134
2135 static void
2136 handle_100b01(struct bregs *regs)
2137 {
2138     // XXX - inline
2139     biosfn_set_palette(regs);
2140 }
2141
2142 static void
2143 handle_100bXX(struct bregs *regs)
2144 {
2145     debug_stub(regs);
2146 }
2147
2148 static void
2149 handle_100b(struct bregs *regs)
2150 {
2151     switch (regs->bh) {
2152     case 0x00: handle_100b00(regs); break;
2153     case 0x01: handle_100b01(regs); break;
2154     default:   handle_100bXX(regs); break;
2155     }
2156 }
2157
2158
2159 static void
2160 handle_100c(struct bregs *regs)
2161 {
2162     // XXX - inline
2163     biosfn_write_pixel(regs->bh, regs->al, regs->cx, regs->dx);
2164 }
2165
2166 static void
2167 handle_100d(struct bregs *regs)
2168 {
2169     // XXX - inline
2170     biosfn_read_pixel(regs->bh, regs->cx, regs->dx, &regs->ax);
2171 }
2172
2173 static void
2174 handle_100e(struct bregs *regs)
2175 {
2176     // Ralf Brown Interrupt list is WRONG on bh(page)
2177     // We do output only on the current page !
2178     biosfn_write_teletype(regs->al, 0xff, regs->bl, NO_ATTR);
2179 }
2180
2181 static void
2182 handle_100f(struct bregs *regs)
2183 {
2184     // XXX - inline
2185     biosfn_get_video_mode(regs);
2186 }
2187
2188
2189 static void
2190 handle_101000(struct bregs *regs)
2191 {
2192     if (regs->bl > 0x14)
2193         return;
2194     biosfn_set_single_palette_reg(regs->bl, regs->bh);
2195 }
2196
2197 static void
2198 handle_101001(struct bregs *regs)
2199 {
2200     // XXX - inline
2201     biosfn_set_overscan_border_color(regs);
2202 }
2203
2204 static void
2205 handle_101002(struct bregs *regs)
2206 {
2207     // XXX - inline
2208     biosfn_set_all_palette_reg(regs);
2209 }
2210
2211 static void
2212 handle_101003(struct bregs *regs)
2213 {
2214     // XXX - inline
2215     biosfn_toggle_intensity(regs);
2216 }
2217
2218 static void
2219 handle_101007(struct bregs *regs)
2220 {
2221     if (regs->bl > 0x14)
2222         return;
2223     regs->bh = biosfn_get_single_palette_reg(regs->bl);
2224 }
2225
2226 static void
2227 handle_101008(struct bregs *regs)
2228 {
2229     // XXX - inline
2230     biosfn_read_overscan_border_color(regs);
2231 }
2232
2233 static void
2234 handle_101009(struct bregs *regs)
2235 {
2236     // XXX - inline
2237     biosfn_get_all_palette_reg(regs);
2238 }
2239
2240 static void
2241 handle_101010(struct bregs *regs)
2242 {
2243     // XXX - inline
2244     biosfn_set_single_dac_reg(regs);
2245 }
2246
2247 static void
2248 handle_101012(struct bregs *regs)
2249 {
2250     // XXX - inline
2251     biosfn_set_all_dac_reg(regs);
2252 }
2253
2254 static void
2255 handle_101013(struct bregs *regs)
2256 {
2257     // XXX - inline
2258     biosfn_select_video_dac_color_page(regs);
2259 }
2260
2261 static void
2262 handle_101015(struct bregs *regs)
2263 {
2264     // XXX - inline
2265     biosfn_read_single_dac_reg(regs);
2266 }
2267
2268 static void
2269 handle_101017(struct bregs *regs)
2270 {
2271     // XXX - inline
2272     biosfn_read_all_dac_reg(regs);
2273 }
2274
2275 static void
2276 handle_101018(struct bregs *regs)
2277 {
2278     // XXX - inline
2279     biosfn_set_pel_mask(regs);
2280 }
2281
2282 static void
2283 handle_101019(struct bregs *regs)
2284 {
2285     // XXX - inline
2286     biosfn_read_pel_mask(regs);
2287 }
2288
2289 static void
2290 handle_10101a(struct bregs *regs)
2291 {
2292     // XXX - inline
2293     biosfn_read_video_dac_state(regs);
2294 }
2295
2296 static void
2297 handle_10101b(struct bregs *regs)
2298 {
2299     biosfn_perform_gray_scale_summing(regs->bx, regs->cx);
2300 }
2301
2302 static void
2303 handle_1010XX(struct bregs *regs)
2304 {
2305     debug_stub(regs);
2306 }
2307
2308 static void
2309 handle_1010(struct bregs *regs)
2310 {
2311     switch (regs->al) {
2312     case 0x00: handle_101000(regs); break;
2313     case 0x01: handle_101001(regs); break;
2314     case 0x02: handle_101002(regs); break;
2315     case 0x03: handle_101003(regs); break;
2316     case 0x07: handle_101007(regs); break;
2317     case 0x08: handle_101008(regs); break;
2318     case 0x09: handle_101009(regs); break;
2319     case 0x10: handle_101010(regs); break;
2320     case 0x12: handle_101012(regs); break;
2321     case 0x13: handle_101013(regs); break;
2322     case 0x15: handle_101015(regs); break;
2323     case 0x17: handle_101017(regs); break;
2324     case 0x18: handle_101018(regs); break;
2325     case 0x19: handle_101019(regs); break;
2326     case 0x1a: handle_10101a(regs); break;
2327     case 0x1b: handle_10101b(regs); break;
2328     default:   handle_1010XX(regs); break;
2329     }
2330 }
2331
2332
2333 static void
2334 handle_101100(struct bregs *regs)
2335 {
2336     // XXX - inline
2337     biosfn_load_text_user_pat(regs->al, regs->es, 0 // XXX - regs->bp
2338                               , regs->cx, regs->dx, regs->bl, regs->bh);
2339 }
2340
2341 static void
2342 handle_101101(struct bregs *regs)
2343 {
2344     // XXX - inline
2345     biosfn_load_text_8_14_pat(regs->al, regs->bl);
2346 }
2347
2348 static void
2349 handle_101102(struct bregs *regs)
2350 {
2351     // XXX - inline
2352     biosfn_load_text_8_8_pat(regs->al, regs->bl);
2353 }
2354
2355 static void
2356 handle_101103(struct bregs *regs)
2357 {
2358     // XXX - inline
2359     biosfn_set_text_block_specifier(regs);
2360 }
2361
2362 static void
2363 handle_101104(struct bregs *regs)
2364 {
2365     // XXX - inline
2366     biosfn_load_text_8_16_pat(regs->al, regs->bl);
2367 }
2368
2369 static void
2370 handle_101110(struct bregs *regs)
2371 {
2372     handle_101100(regs);
2373 }
2374
2375 static void
2376 handle_101111(struct bregs *regs)
2377 {
2378     handle_101101(regs);
2379 }
2380
2381 static void
2382 handle_101112(struct bregs *regs)
2383 {
2384     handle_101102(regs);
2385 }
2386
2387 static void
2388 handle_101114(struct bregs *regs)
2389 {
2390     handle_101104(regs);
2391 }
2392
2393 static void
2394 handle_101130(struct bregs *regs)
2395 {
2396     // XXX - inline
2397     biosfn_get_font_info(regs->bh, &regs->es, 0 // &regs->bp
2398                          , &regs->cx, &regs->dx);
2399 }
2400
2401 static void
2402 handle_1011XX(struct bregs *regs)
2403 {
2404     debug_stub(regs);
2405 }
2406
2407 static void
2408 handle_1011(struct bregs *regs)
2409 {
2410     switch (regs->al) {
2411     case 0x00: handle_101100(regs); break;
2412     case 0x01: handle_101101(regs); break;
2413     case 0x02: handle_101102(regs); break;
2414     case 0x03: handle_101103(regs); break;
2415     case 0x04: handle_101104(regs); break;
2416     case 0x10: handle_101110(regs); break;
2417     case 0x11: handle_101111(regs); break;
2418     case 0x12: handle_101112(regs); break;
2419     case 0x14: handle_101114(regs); break;
2420     case 0x30: handle_101130(regs); break;
2421     default:   handle_1011XX(regs); break;
2422     }
2423 }
2424
2425
2426 static void
2427 handle_101210(struct bregs *regs)
2428 {
2429     // XXX - inline
2430     biosfn_get_ega_info(regs);
2431 }
2432
2433 static void
2434 handle_101230(struct bregs *regs)
2435 {
2436     // XXX - inline
2437     biosfn_select_vert_res(regs);
2438 }
2439
2440 static void
2441 handle_101231(struct bregs *regs)
2442 {
2443     // XXX - inline
2444     biosfn_enable_default_palette_loading(regs);
2445 }
2446
2447 static void
2448 handle_101232(struct bregs *regs)
2449 {
2450     // XXX - inline
2451     biosfn_enable_video_addressing(regs);
2452 }
2453
2454 static void
2455 handle_101233(struct bregs *regs)
2456 {
2457     // XXX - inline
2458     biosfn_enable_grayscale_summing(regs);
2459 }
2460
2461 static void
2462 handle_101234(struct bregs *regs)
2463 {
2464     // XXX - inline
2465     biosfn_enable_cursor_emulation(regs);
2466 }
2467
2468 static void
2469 handle_101235(struct bregs *regs)
2470 {
2471     debug_stub(regs);
2472     regs->al = 0x12;
2473 }
2474
2475 static void
2476 handle_101236(struct bregs *regs)
2477 {
2478     debug_stub(regs);
2479     regs->al = 0x12;
2480 }
2481
2482 static void
2483 handle_1012XX(struct bregs *regs)
2484 {
2485     debug_stub(regs);
2486 }
2487
2488 static void
2489 handle_1012(struct bregs *regs)
2490 {
2491     switch (regs->bl) {
2492     case 0x10: handle_101210(regs); break;
2493     case 0x30: handle_101230(regs); break;
2494     case 0x31: handle_101231(regs); break;
2495     case 0x32: handle_101232(regs); break;
2496     case 0x33: handle_101233(regs); break;
2497     case 0x34: handle_101234(regs); break;
2498     case 0x35: handle_101235(regs); break;
2499     case 0x36: handle_101236(regs); break;
2500     default:   handle_1012XX(regs); break;
2501     }
2502
2503     // XXX - cirrus has 1280, 1281, 1282, 1285, 129a, 12a0, 12a1, 12a2, 12ae
2504 }
2505
2506
2507 static void
2508 handle_1013(struct bregs *regs)
2509 {
2510     // XXX - inline
2511     biosfn_write_string(regs->al, regs->bh, regs->bl, regs->cx
2512                         , regs->dh, regs->dl, regs->es, 0); // regs->bp);
2513 }
2514
2515
2516 static void
2517 handle_101a00(struct bregs *regs)
2518 {
2519     // XXX - inline
2520     biosfn_read_display_code(regs);
2521 }
2522
2523 static void
2524 handle_101a01(struct bregs *regs)
2525 {
2526     // XXX - inline
2527     biosfn_set_display_code(regs);
2528 }
2529
2530 static void
2531 handle_101aXX(struct bregs *regs)
2532 {
2533     debug_stub(regs);
2534 }
2535
2536 static void
2537 handle_101a(struct bregs *regs)
2538 {
2539     switch (regs->al) {
2540     case 0x00: handle_101a00(regs); break;
2541     case 0x01: handle_101a01(regs); break;
2542     default:   handle_101aXX(regs); break;
2543     }
2544 }
2545
2546
2547 static void
2548 handle_101b(struct bregs *regs)
2549 {
2550     // XXX - inline
2551     biosfn_read_state_info(regs->bx, regs->es, regs->di);
2552     regs->al = 0x1B;
2553 }
2554
2555
2556 static void
2557 handle_101c00(struct bregs *regs)
2558 {
2559     // XXX - inline
2560     regs->bx = biosfn_read_video_state_size(regs->cx);
2561 }
2562
2563 static void
2564 handle_101c01(struct bregs *regs)
2565 {
2566     // XXX - inline
2567     biosfn_save_video_state(regs->cx, regs->es, regs->bx);
2568 }
2569
2570 static void
2571 handle_101c02(struct bregs *regs)
2572 {
2573     // XXX - inline
2574     biosfn_restore_video_state(regs->cx, regs->es, regs->bx);
2575 }
2576
2577 static void
2578 handle_101cXX(struct bregs *regs)
2579 {
2580     debug_stub(regs);
2581 }
2582
2583 static void
2584 handle_101c(struct bregs *regs)
2585 {
2586     switch (regs->al) {
2587     case 0x00: handle_101c00(regs); break;
2588     case 0x01: handle_101c01(regs); break;
2589     case 0x02: handle_101c02(regs); break;
2590     default:   handle_101cXX(regs); break;
2591     }
2592 }
2593
2594
2595 static void
2596 handle_104f00(struct bregs *regs)
2597 {
2598     // XXX - vbe_biosfn_return_controller_information(&AX,ES,DI);
2599     // XXX - OR cirrus_vesa_00h
2600 }
2601
2602 static void
2603 handle_104f01(struct bregs *regs)
2604 {
2605     // XXX - vbe_biosfn_return_mode_information(&AX,CX,ES,DI);
2606     // XXX - OR cirrus_vesa_01h
2607 }
2608
2609 static void
2610 handle_104f02(struct bregs *regs)
2611 {
2612     // XXX - vbe_biosfn_set_mode(&AX,BX,ES,DI);
2613     // XXX - OR cirrus_vesa_02h
2614 }
2615
2616 static void
2617 handle_104f03(struct bregs *regs)
2618 {
2619     // XXX - vbe_biosfn_return_current_mode
2620     // XXX - OR cirrus_vesa_03h
2621 }
2622
2623 static void
2624 handle_104f04(struct bregs *regs)
2625 {
2626     // XXX - vbe_biosfn_save_restore_state(&AX, CX, DX, ES, &BX);
2627 }
2628
2629 static void
2630 handle_104f05(struct bregs *regs)
2631 {
2632     // XXX - vbe_biosfn_display_window_control
2633     // XXX - OR cirrus_vesa_05h
2634 }
2635
2636 static void
2637 handle_104f06(struct bregs *regs)
2638 {
2639     // XXX - vbe_biosfn_set_get_logical_scan_line_length
2640     // XXX - OR cirrus_vesa_06h
2641 }
2642
2643 static void
2644 handle_104f07(struct bregs *regs)
2645 {
2646     // XXX - vbe_biosfn_set_get_display_start
2647     // XXX - OR cirrus_vesa_07h
2648 }
2649
2650 static void
2651 handle_104f08(struct bregs *regs)
2652 {
2653     // XXX - vbe_biosfn_set_get_dac_palette_format
2654 }
2655
2656 static void
2657 handle_104f0a(struct bregs *regs)
2658 {
2659     // XXX - vbe_biosfn_return_protected_mode_interface
2660 }
2661
2662 static void
2663 handle_104fXX(struct bregs *regs)
2664 {
2665     debug_stub(regs);
2666     regs->ax = 0x0100;
2667 }
2668
2669 static void
2670 handle_104f(struct bregs *regs)
2671 {
2672     if (! CONFIG_VBE) {
2673         handle_104fXX(regs);
2674         return;
2675     }
2676
2677     // XXX - check vbe_has_vbe_display()?
2678
2679     switch (regs->al) {
2680     case 0x00: handle_104f00(regs); break;
2681     case 0x01: handle_104f01(regs); break;
2682     case 0x02: handle_104f02(regs); break;
2683     case 0x03: handle_104f03(regs); break;
2684     case 0x04: handle_104f04(regs); break;
2685     case 0x05: handle_104f05(regs); break;
2686     case 0x06: handle_104f06(regs); break;
2687     case 0x07: handle_104f07(regs); break;
2688     case 0x08: handle_104f08(regs); break;
2689     case 0x0a: handle_104f0a(regs); break;
2690     default:   handle_104fXX(regs); break;
2691     }
2692 }
2693
2694
2695 static void
2696 handle_10XX(struct bregs *regs)
2697 {
2698     debug_stub(regs);
2699 }
2700
2701 // INT 10h Video Support Service Entry Point
2702 void VISIBLE16
2703 handle_10(struct bregs *regs)
2704 {
2705     debug_enter(regs, DEBUG_VGA_10);
2706     switch (regs->ah) {
2707     case 0x00: handle_1000(regs); break;
2708     case 0x01: handle_1001(regs); break;
2709     case 0x02: handle_1002(regs); break;
2710     case 0x03: handle_1003(regs); break;
2711     case 0x04: handle_1004(regs); break;
2712     case 0x05: handle_1005(regs); break;
2713     case 0x06: handle_1006(regs); break;
2714     case 0x07: handle_1007(regs); break;
2715     case 0x08: handle_1008(regs); break;
2716     case 0x09: handle_1009(regs); break;
2717     case 0x0a: handle_100a(regs); break;
2718     case 0x0b: handle_100b(regs); break;
2719     case 0x0c: handle_100c(regs); break;
2720     case 0x0d: handle_100d(regs); break;
2721     case 0x0e: handle_100e(regs); break;
2722     case 0x0f: handle_100f(regs); break;
2723     case 0x10: handle_1010(regs); break;
2724     case 0x11: handle_1011(regs); break;
2725     case 0x12: handle_1012(regs); break;
2726     case 0x13: handle_1013(regs); break;
2727     case 0x1a: handle_101a(regs); break;
2728     case 0x1b: handle_101b(regs); break;
2729     case 0x1c: handle_101c(regs); break;
2730     case 0x4f: handle_104f(regs); break;
2731     default:   handle_10XX(regs); break;
2732     }
2733 }
2734
2735
2736 /****************************************************************
2737  * VGA post
2738  ****************************************************************/
2739
2740 static void
2741 init_bios_area()
2742 {
2743     // init detected hardware BIOS Area
2744     // set 80x25 color (not clear from RBIL but usual)
2745     u16 eqf = GET_BDA(equipment_list_flags);
2746     SET_BDA(equipment_list_flags, (eqf & 0xffcf) | 0x20);
2747
2748     // Just for the first int10 find its children
2749
2750     // the default char height
2751     SET_BDA(char_height, 0x10);
2752
2753     // Clear the screen
2754     SET_BDA(video_ctl, 0x60);
2755
2756     // Set the basic screen we have
2757     SET_BDA(video_switches, 0xf9);
2758
2759     // Set the basic modeset options
2760     SET_BDA(modeset_ctl, 0x51);
2761
2762     // Set the  default MSR
2763     SET_BDA(video_msr, 0x09);
2764 }
2765
2766 static void
2767 init_vga_card()
2768 {
2769     // switch to color mode and enable CPU access 480 lines
2770     outb(0xc3, VGAREG_WRITE_MISC_OUTPUT);
2771     // more than 64k 3C4/04
2772     outb(0x04, VGAREG_SEQU_ADDRESS);
2773     outb(0x02, VGAREG_SEQU_DATA);
2774 }
2775
2776 void VISIBLE16
2777 vga_post(struct bregs *regs)
2778 {
2779     debug_enter(regs, DEBUG_VGA_POST);
2780
2781     init_vga_card();
2782
2783     init_bios_area();
2784
2785     // vbe_init();
2786
2787     extern void entry_10(void);
2788     SET_IVT(0x10, 0xC000, (u32)entry_10);
2789
2790     if (CONFIG_CIRRUS)
2791         cirrus_init();
2792
2793     // XXX - clear screen and display info
2794
2795     // XXX: fill it
2796     SET_VGA(video_save_pointer_table[0], (u32)video_param_table);
2797     SET_VGA(video_save_pointer_table[1], 0xC000);
2798
2799     // Fixup checksum
2800     extern u8 _rom_header_size, _rom_header_checksum;
2801     SET_VGA(_rom_header_checksum, 0);
2802     u8 sum = -checksum_far(0xC000, 0, _rom_header_size * 512);
2803     SET_VGA(_rom_header_checksum, sum);
2804 }