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