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