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