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