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