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, *(u32*)(BX+0), GET_IVT(0x1f).segoff);
1765         BX += 4;
1766         SET_FARVAR(ES, *(u32*)(BX+0), GET_IVT(0x43).segoff);
1767         BX += 4;
1768     }
1769     if (CX & 4) {
1770         /* XXX: check this */
1771         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_STATE));
1772         BX++;                   /* read/write mode dac */
1773         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_WRITE_ADDRESS));
1774         BX++;                   /* pix address */
1775         SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_PEL_MASK));
1776         BX++;
1777         // Set the whole dac always, from 0
1778         outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
1779         u16 i;
1780         for (i = 0; i < 256 * 3; i++) {
1781             SET_FARVAR(ES, *(u8*)(BX+0), inb(VGAREG_DAC_DATA));
1782             BX++;
1783         }
1784         SET_FARVAR(ES, *(u8*)(BX+0), 0);
1785         BX++;                   /* color select register */
1786     }
1787     return BX;
1788 }
1789
1790 static u16
1791 biosfn_restore_video_state(u16 CX, u16 ES, u16 BX)
1792 {
1793     if (CX & 1) {
1794         // Reset Attribute Ctl flip-flop
1795         inb(VGAREG_ACTL_RESET);
1796
1797         u16 crtc_addr = GET_FARVAR(ES, *(u16*)(BX + 0x40));
1798         u16 addr1 = BX;
1799         BX += 5;
1800
1801         u16 i;
1802         for (i = 1; i <= 4; i++) {
1803             outb(i, VGAREG_SEQU_ADDRESS);
1804             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_SEQU_DATA);
1805             BX++;
1806         }
1807         outb(0, VGAREG_SEQU_ADDRESS);
1808         outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_SEQU_DATA);
1809         BX++;
1810
1811         // Disable CRTC write protection
1812         outw(0x0011, crtc_addr);
1813         // Set CRTC regs
1814         for (i = 0; i <= 0x18; i++) {
1815             if (i != 0x11) {
1816                 outb(i, crtc_addr);
1817                 outb(GET_FARVAR(ES, *(u8*)(BX+0)), crtc_addr + 1);
1818             }
1819             BX++;
1820         }
1821         // select crtc base address
1822         u16 v = inb(VGAREG_READ_MISC_OUTPUT) & ~0x01;
1823         if (crtc_addr == VGAREG_VGA_CRTC_ADDRESS)
1824             v |= 0x01;
1825         outb(v, VGAREG_WRITE_MISC_OUTPUT);
1826
1827         // enable write protection if needed
1828         outb(0x11, crtc_addr);
1829         outb(GET_FARVAR(ES, *(u8*)(BX - 0x18 + 0x11)), crtc_addr + 1);
1830
1831         // Set Attribute Ctl
1832         u16 ar_index = GET_FARVAR(ES, *(u8*)(addr1 + 0x03));
1833         inb(VGAREG_ACTL_RESET);
1834         for (i = 0; i <= 0x13; i++) {
1835             outb(i | (ar_index & 0x20), VGAREG_ACTL_ADDRESS);
1836             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_ACTL_WRITE_DATA);
1837             BX++;
1838         }
1839         outb(ar_index, VGAREG_ACTL_ADDRESS);
1840         inb(VGAREG_ACTL_RESET);
1841
1842         for (i = 0; i <= 8; i++) {
1843             outb(i, VGAREG_GRDC_ADDRESS);
1844             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_GRDC_DATA);
1845             BX++;
1846         }
1847         BX += 2;                /* crtc_addr */
1848         BX += 4;                /* plane latches */
1849
1850         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), VGAREG_SEQU_ADDRESS);
1851         addr1++;
1852         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), crtc_addr);
1853         addr1++;
1854         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), VGAREG_GRDC_ADDRESS);
1855         addr1++;
1856         addr1++;
1857         outb(GET_FARVAR(ES, *(u8*)(addr1+0)), crtc_addr - 0x4 + 0xa);
1858         addr1++;
1859     }
1860     if (CX & 2) {
1861         SET_BDA(video_mode, GET_FARVAR(ES, *(u8*)(BX+0)));
1862         BX++;
1863         SET_BDA(video_cols, GET_FARVAR(ES, *(u16*)(BX+0)));
1864         BX += 2;
1865         SET_BDA(video_pagesize, GET_FARVAR(ES, *(u16*)(BX+0)));
1866         BX += 2;
1867         SET_BDA(crtc_address, GET_FARVAR(ES, *(u16*)(BX+0)));
1868         BX += 2;
1869         SET_BDA(video_rows, GET_FARVAR(ES, *(u8*)(BX+0)));
1870         BX++;
1871         SET_BDA(char_height, GET_FARVAR(ES, *(u16*)(BX+0)));
1872         BX += 2;
1873         SET_BDA(video_ctl, GET_FARVAR(ES, *(u8*)(BX+0)));
1874         BX++;
1875         SET_BDA(video_switches, GET_FARVAR(ES, *(u8*)(BX+0)));
1876         BX++;
1877         SET_BDA(modeset_ctl, GET_FARVAR(ES, *(u8*)(BX+0)));
1878         BX++;
1879         SET_BDA(cursor_type, GET_FARVAR(ES, *(u16*)(BX+0)));
1880         BX += 2;
1881         u16 i;
1882         for (i = 0; i < 8; i++) {
1883             SET_BDA(cursor_pos[i], GET_FARVAR(ES, *(u16*)(BX+0)));
1884             BX += 2;
1885         }
1886         SET_BDA(video_pagestart, GET_FARVAR(ES, *(u16*)(BX+0)));
1887         BX += 2;
1888         SET_BDA(video_page, GET_FARVAR(ES, *(u8*)(BX+0)));
1889         BX++;
1890         /* current font */
1891         SET_IVT(0x1f, GET_FARVAR(ES, *(u16*)(BX+2)), GET_FARVAR(ES, *(u16*)(BX+0)));
1892         BX += 4;
1893         SET_IVT(0x43, GET_FARVAR(ES, *(u16*)(BX+2)), GET_FARVAR(ES, *(u16*)(BX+0)));
1894         BX += 4;
1895     }
1896     if (CX & 4) {
1897         BX++;
1898         u16 v = GET_FARVAR(ES, *(u8*)(BX+0));
1899         BX++;
1900         outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_PEL_MASK);
1901         BX++;
1902         // Set the whole dac always, from 0
1903         outb(0x00, VGAREG_DAC_WRITE_ADDRESS);
1904         u16 i;
1905         for (i = 0; i < 256 * 3; i++) {
1906             outb(GET_FARVAR(ES, *(u8*)(BX+0)), VGAREG_DAC_DATA);
1907             BX++;
1908         }
1909         BX++;
1910         outb(v, VGAREG_DAC_WRITE_ADDRESS);
1911     }
1912     return BX;
1913 }
1914
1915
1916 /****************************************************************
1917  * VGA int 10 handler
1918  ****************************************************************/
1919
1920 static void
1921 handle_1000(struct bregs *regs)
1922 {
1923     // XXX - inline
1924     biosfn_set_video_mode(regs->al);
1925     switch(regs->al & 0x7F) {
1926     case 6:
1927         regs->al = 0x3F;
1928         break;
1929     case 0:
1930     case 1:
1931     case 2:
1932     case 3:
1933     case 4:
1934     case 5:
1935     case 7:
1936         regs->al = 0x30;
1937         break;
1938     default:
1939         regs->al = 0x20;
1940     }
1941 }
1942
1943 static void
1944 handle_1001(struct bregs *regs)
1945 {
1946     biosfn_set_cursor_shape(regs->ch, regs->cl);
1947 }
1948
1949 static void
1950 handle_1002(struct bregs *regs)
1951 {
1952     biosfn_set_cursor_pos(regs->bh, regs->dx);
1953 }
1954
1955 static void
1956 handle_1003(struct bregs *regs)
1957 {
1958     regs->cx = biosfn_get_cursor_shape(regs->bh);
1959     regs->dx = biosfn_get_cursor_pos(regs->bh);
1960 }
1961
1962 // Read light pen pos (unimplemented)
1963 static void
1964 handle_1004(struct bregs *regs)
1965 {
1966     debug_stub(regs);
1967     regs->ax = regs->bx = regs->cx = regs->dx = 0;
1968 }
1969
1970 static void
1971 handle_1005(struct bregs *regs)
1972 {
1973     biosfn_set_active_page(regs->al);
1974 }
1975
1976 static void
1977 handle_1006(struct bregs *regs)
1978 {
1979     biosfn_scroll(regs->al, regs->bh, regs->ch, regs->cl, regs->dh, regs->dl
1980                   , 0xFF, SCROLL_UP);
1981 }
1982
1983 static void
1984 handle_1007(struct bregs *regs)
1985 {
1986     biosfn_scroll(regs->al, regs->bh, regs->ch, regs->cl, regs->dh, regs->dl
1987                   , 0xFF, SCROLL_DOWN);
1988 }
1989
1990 static void
1991 handle_1008(struct bregs *regs)
1992 {
1993     // XXX - inline
1994     biosfn_read_char_attr(regs->bh, &regs->ax);
1995 }
1996
1997 static void
1998 handle_1009(struct bregs *regs)
1999 {
2000     // XXX - inline
2001     biosfn_write_char_attr(regs->al, regs->bh, regs->bl, regs->cx);
2002 }
2003
2004 static void
2005 handle_100a(struct bregs *regs)
2006 {
2007     // XXX - inline
2008     biosfn_write_char_only(regs->al, regs->bh, regs->bl, regs->cx);
2009 }
2010
2011
2012 static void
2013 handle_100b00(struct bregs *regs)
2014 {
2015     // XXX - inline
2016     biosfn_set_border_color(regs);
2017 }
2018
2019 static void
2020 handle_100b01(struct bregs *regs)
2021 {
2022     // XXX - inline
2023     biosfn_set_palette(regs);
2024 }
2025
2026 static void
2027 handle_100bXX(struct bregs *regs)
2028 {
2029     debug_stub(regs);
2030 }
2031
2032 static void
2033 handle_100b(struct bregs *regs)
2034 {
2035     switch (regs->bh) {
2036     case 0x00: handle_100b00(regs); break;
2037     case 0x01: handle_100b01(regs); break;
2038     default:   handle_100bXX(regs); break;
2039     }
2040 }
2041
2042
2043 static void
2044 handle_100c(struct bregs *regs)
2045 {
2046     // XXX - inline
2047     biosfn_write_pixel(regs->bh, regs->al, regs->cx, regs->dx);
2048 }
2049
2050 static void
2051 handle_100d(struct bregs *regs)
2052 {
2053     // XXX - inline
2054     biosfn_read_pixel(regs->bh, regs->cx, regs->dx, &regs->ax);
2055 }
2056
2057 static void
2058 handle_100e(struct bregs *regs)
2059 {
2060     // Ralf Brown Interrupt list is WRONG on bh(page)
2061     // We do output only on the current page !
2062     biosfn_write_teletype(regs->al, 0xff, regs->bl, NO_ATTR);
2063 }
2064
2065 static void
2066 handle_100f(struct bregs *regs)
2067 {
2068     // XXX - inline
2069     biosfn_get_video_mode(regs);
2070 }
2071
2072
2073 static void
2074 handle_101000(struct bregs *regs)
2075 {
2076     if (regs->bl > 0x14)
2077         return;
2078     biosfn_set_single_palette_reg(regs->bl, regs->bh);
2079 }
2080
2081 static void
2082 handle_101001(struct bregs *regs)
2083 {
2084     // XXX - inline
2085     biosfn_set_overscan_border_color(regs);
2086 }
2087
2088 static void
2089 handle_101002(struct bregs *regs)
2090 {
2091     // XXX - inline
2092     biosfn_set_all_palette_reg(regs);
2093 }
2094
2095 static void
2096 handle_101003(struct bregs *regs)
2097 {
2098     // XXX - inline
2099     biosfn_toggle_intensity(regs);
2100 }
2101
2102 static void
2103 handle_101007(struct bregs *regs)
2104 {
2105     if (regs->bl > 0x14)
2106         return;
2107     regs->bh = biosfn_get_single_palette_reg(regs->bl);
2108 }
2109
2110 static void
2111 handle_101008(struct bregs *regs)
2112 {
2113     // XXX - inline
2114     biosfn_read_overscan_border_color(regs);
2115 }
2116
2117 static void
2118 handle_101009(struct bregs *regs)
2119 {
2120     // XXX - inline
2121     biosfn_get_all_palette_reg(regs);
2122 }
2123
2124 static void
2125 handle_101010(struct bregs *regs)
2126 {
2127     // XXX - inline
2128     biosfn_set_single_dac_reg(regs);
2129 }
2130
2131 static void
2132 handle_101012(struct bregs *regs)
2133 {
2134     // XXX - inline
2135     biosfn_set_all_dac_reg(regs);
2136 }
2137
2138 static void
2139 handle_101013(struct bregs *regs)
2140 {
2141     // XXX - inline
2142     biosfn_select_video_dac_color_page(regs);
2143 }
2144
2145 static void
2146 handle_101015(struct bregs *regs)
2147 {
2148     // XXX - inline
2149     biosfn_read_single_dac_reg(regs);
2150 }
2151
2152 static void
2153 handle_101017(struct bregs *regs)
2154 {
2155     // XXX - inline
2156     biosfn_read_all_dac_reg(regs);
2157 }
2158
2159 static void
2160 handle_101018(struct bregs *regs)
2161 {
2162     // XXX - inline
2163     biosfn_set_pel_mask(regs);
2164 }
2165
2166 static void
2167 handle_101019(struct bregs *regs)
2168 {
2169     // XXX - inline
2170     biosfn_read_pel_mask(regs);
2171 }
2172
2173 static void
2174 handle_10101a(struct bregs *regs)
2175 {
2176     // XXX - inline
2177     biosfn_read_video_dac_state(regs);
2178 }
2179
2180 static void
2181 handle_10101b(struct bregs *regs)
2182 {
2183     biosfn_perform_gray_scale_summing(regs->bx, regs->cx);
2184 }
2185
2186 static void
2187 handle_1010XX(struct bregs *regs)
2188 {
2189     debug_stub(regs);
2190 }
2191
2192 static void
2193 handle_1010(struct bregs *regs)
2194 {
2195     switch (regs->al) {
2196     case 0x00: handle_101000(regs); break;
2197     case 0x01: handle_101001(regs); break;
2198     case 0x02: handle_101002(regs); break;
2199     case 0x03: handle_101003(regs); break;
2200     case 0x07: handle_101007(regs); break;
2201     case 0x08: handle_101008(regs); break;
2202     case 0x09: handle_101009(regs); break;
2203     case 0x10: handle_101010(regs); break;
2204     case 0x12: handle_101012(regs); break;
2205     case 0x13: handle_101013(regs); break;
2206     case 0x15: handle_101015(regs); break;
2207     case 0x17: handle_101017(regs); break;
2208     case 0x18: handle_101018(regs); break;
2209     case 0x19: handle_101019(regs); break;
2210     case 0x1a: handle_10101a(regs); break;
2211     case 0x1b: handle_10101b(regs); break;
2212     default:   handle_1010XX(regs); break;
2213     }
2214 }
2215
2216
2217 static void
2218 handle_101100(struct bregs *regs)
2219 {
2220     // XXX - inline
2221     biosfn_load_text_user_pat(regs->al, regs->es, 0 // XXX - regs->bp
2222                               , regs->cx, regs->dx, regs->bl, regs->bh);
2223 }
2224
2225 static void
2226 handle_101101(struct bregs *regs)
2227 {
2228     // XXX - inline
2229     biosfn_load_text_8_14_pat(regs->al, regs->bl);
2230 }
2231
2232 static void
2233 handle_101102(struct bregs *regs)
2234 {
2235     // XXX - inline
2236     biosfn_load_text_8_8_pat(regs->al, regs->bl);
2237 }
2238
2239 static void
2240 handle_101103(struct bregs *regs)
2241 {
2242     // XXX - inline
2243     biosfn_set_text_block_specifier(regs);
2244 }
2245
2246 static void
2247 handle_101104(struct bregs *regs)
2248 {
2249     // XXX - inline
2250     biosfn_load_text_8_16_pat(regs->al, regs->bl);
2251 }
2252
2253 static void
2254 handle_101110(struct bregs *regs)
2255 {
2256     handle_101100(regs);
2257 }
2258
2259 static void
2260 handle_101111(struct bregs *regs)
2261 {
2262     handle_101101(regs);
2263 }
2264
2265 static void
2266 handle_101112(struct bregs *regs)
2267 {
2268     handle_101102(regs);
2269 }
2270
2271 static void
2272 handle_101114(struct bregs *regs)
2273 {
2274     handle_101104(regs);
2275 }
2276
2277 static void
2278 handle_101130(struct bregs *regs)
2279 {
2280     // XXX - inline
2281     biosfn_get_font_info(regs->bh, &regs->es, 0 // &regs->bp
2282                          , &regs->cx, &regs->dx);
2283 }
2284
2285 static void
2286 handle_1011XX(struct bregs *regs)
2287 {
2288     debug_stub(regs);
2289 }
2290
2291 static void
2292 handle_1011(struct bregs *regs)
2293 {
2294     switch (regs->al) {
2295     case 0x00: handle_101100(regs); break;
2296     case 0x01: handle_101101(regs); break;
2297     case 0x02: handle_101102(regs); break;
2298     case 0x03: handle_101103(regs); break;
2299     case 0x04: handle_101104(regs); break;
2300     case 0x10: handle_101110(regs); break;
2301     case 0x11: handle_101111(regs); break;
2302     case 0x12: handle_101112(regs); break;
2303     case 0x14: handle_101114(regs); break;
2304     case 0x30: handle_101130(regs); break;
2305     default:   handle_1011XX(regs); break;
2306     }
2307 }
2308
2309
2310 static void
2311 handle_101210(struct bregs *regs)
2312 {
2313     // XXX - inline
2314     biosfn_get_ega_info(regs);
2315 }
2316
2317 static void
2318 handle_101230(struct bregs *regs)
2319 {
2320     // XXX - inline
2321     biosfn_select_vert_res(regs);
2322 }
2323
2324 static void
2325 handle_101231(struct bregs *regs)
2326 {
2327     // XXX - inline
2328     biosfn_enable_default_palette_loading(regs);
2329 }
2330
2331 static void
2332 handle_101232(struct bregs *regs)
2333 {
2334     // XXX - inline
2335     biosfn_enable_video_addressing(regs);
2336 }
2337
2338 static void
2339 handle_101233(struct bregs *regs)
2340 {
2341     // XXX - inline
2342     biosfn_enable_grayscale_summing(regs);
2343 }
2344
2345 static void
2346 handle_101234(struct bregs *regs)
2347 {
2348     // XXX - inline
2349     biosfn_enable_cursor_emulation(regs);
2350 }
2351
2352 static void
2353 handle_101235(struct bregs *regs)
2354 {
2355     debug_stub(regs);
2356     regs->al = 0x12;
2357 }
2358
2359 static void
2360 handle_101236(struct bregs *regs)
2361 {
2362     debug_stub(regs);
2363     regs->al = 0x12;
2364 }
2365
2366 static void
2367 handle_1012XX(struct bregs *regs)
2368 {
2369     debug_stub(regs);
2370 }
2371
2372 static void
2373 handle_1012(struct bregs *regs)
2374 {
2375     switch (regs->bl) {
2376     case 0x10: handle_101210(regs); break;
2377     case 0x30: handle_101230(regs); break;
2378     case 0x31: handle_101231(regs); break;
2379     case 0x32: handle_101232(regs); break;
2380     case 0x33: handle_101233(regs); break;
2381     case 0x34: handle_101234(regs); break;
2382     case 0x35: handle_101235(regs); break;
2383     case 0x36: handle_101236(regs); break;
2384     default:   handle_1012XX(regs); break;
2385     }
2386
2387     // XXX - cirrus has 1280, 1281, 1282, 1285, 129a, 12a0, 12a1, 12a2, 12ae
2388 }
2389
2390
2391 static void
2392 handle_1013(struct bregs *regs)
2393 {
2394     // XXX - inline
2395     biosfn_write_string(regs->al, regs->bh, regs->bl, regs->cx
2396                         , regs->dh, regs->dl, regs->es, 0); // regs->bp);
2397 }
2398
2399
2400 static void
2401 handle_101a00(struct bregs *regs)
2402 {
2403     // XXX - inline
2404     biosfn_read_display_code(regs);
2405 }
2406
2407 static void
2408 handle_101a01(struct bregs *regs)
2409 {
2410     // XXX - inline
2411     biosfn_set_display_code(regs);
2412 }
2413
2414 static void
2415 handle_101aXX(struct bregs *regs)
2416 {
2417     debug_stub(regs);
2418 }
2419
2420 static void
2421 handle_101a(struct bregs *regs)
2422 {
2423     switch (regs->al) {
2424     case 0x00: handle_101a00(regs); break;
2425     case 0x01: handle_101a01(regs); break;
2426     default:   handle_101aXX(regs); break;
2427     }
2428 }
2429
2430
2431 static void
2432 handle_101b(struct bregs *regs)
2433 {
2434     // XXX - inline
2435     biosfn_read_state_info(regs->bx, regs->es, regs->di);
2436     regs->al = 0x1B;
2437 }
2438
2439
2440 static void
2441 handle_101c00(struct bregs *regs)
2442 {
2443     // XXX - inline
2444     regs->bx = biosfn_read_video_state_size(regs->cx);
2445 }
2446
2447 static void
2448 handle_101c01(struct bregs *regs)
2449 {
2450     // XXX - inline
2451     biosfn_save_video_state(regs->cx, regs->es, regs->bx);
2452 }
2453
2454 static void
2455 handle_101c02(struct bregs *regs)
2456 {
2457     // XXX - inline
2458     biosfn_restore_video_state(regs->cx, regs->es, regs->bx);
2459 }
2460
2461 static void
2462 handle_101cXX(struct bregs *regs)
2463 {
2464     debug_stub(regs);
2465 }
2466
2467 static void
2468 handle_101c(struct bregs *regs)
2469 {
2470     switch (regs->al) {
2471     case 0x00: handle_101c00(regs); break;
2472     case 0x01: handle_101c01(regs); break;
2473     case 0x02: handle_101c02(regs); break;
2474     default:   handle_101cXX(regs); break;
2475     }
2476 }
2477
2478
2479 static void
2480 handle_104f00(struct bregs *regs)
2481 {
2482     // XXX - vbe_biosfn_return_controller_information(&AX,ES,DI);
2483     // XXX - OR cirrus_vesa_00h
2484 }
2485
2486 static void
2487 handle_104f01(struct bregs *regs)
2488 {
2489     // XXX - vbe_biosfn_return_mode_information(&AX,CX,ES,DI);
2490     // XXX - OR cirrus_vesa_01h
2491 }
2492
2493 static void
2494 handle_104f02(struct bregs *regs)
2495 {
2496     // XXX - vbe_biosfn_set_mode(&AX,BX,ES,DI);
2497     // XXX - OR cirrus_vesa_02h
2498 }
2499
2500 static void
2501 handle_104f03(struct bregs *regs)
2502 {
2503     // XXX - vbe_biosfn_return_current_mode
2504     // XXX - OR cirrus_vesa_03h
2505 }
2506
2507 static void
2508 handle_104f04(struct bregs *regs)
2509 {
2510     // XXX - vbe_biosfn_save_restore_state(&AX, CX, DX, ES, &BX);
2511 }
2512
2513 static void
2514 handle_104f05(struct bregs *regs)
2515 {
2516     // XXX - vbe_biosfn_display_window_control
2517     // XXX - OR cirrus_vesa_05h
2518 }
2519
2520 static void
2521 handle_104f06(struct bregs *regs)
2522 {
2523     // XXX - vbe_biosfn_set_get_logical_scan_line_length
2524     // XXX - OR cirrus_vesa_06h
2525 }
2526
2527 static void
2528 handle_104f07(struct bregs *regs)
2529 {
2530     // XXX - vbe_biosfn_set_get_display_start
2531     // XXX - OR cirrus_vesa_07h
2532 }
2533
2534 static void
2535 handle_104f08(struct bregs *regs)
2536 {
2537     // XXX - vbe_biosfn_set_get_dac_palette_format
2538 }
2539
2540 static void
2541 handle_104f0a(struct bregs *regs)
2542 {
2543     // XXX - vbe_biosfn_return_protected_mode_interface
2544 }
2545
2546 static void
2547 handle_104fXX(struct bregs *regs)
2548 {
2549     debug_stub(regs);
2550     regs->ax = 0x0100;
2551 }
2552
2553 static void
2554 handle_104f(struct bregs *regs)
2555 {
2556     if (! CONFIG_VBE) {
2557         handle_104fXX(regs);
2558         return;
2559     }
2560
2561     // XXX - check vbe_has_vbe_display()?
2562
2563     switch (regs->al) {
2564     case 0x00: handle_104f00(regs); break;
2565     case 0x01: handle_104f01(regs); break;
2566     case 0x02: handle_104f02(regs); break;
2567     case 0x03: handle_104f03(regs); break;
2568     case 0x04: handle_104f04(regs); break;
2569     case 0x05: handle_104f05(regs); break;
2570     case 0x06: handle_104f06(regs); break;
2571     case 0x07: handle_104f07(regs); break;
2572     case 0x08: handle_104f08(regs); break;
2573     case 0x0a: handle_104f0a(regs); break;
2574     default:   handle_104fXX(regs); break;
2575     }
2576 }
2577
2578
2579 static void
2580 handle_10XX(struct bregs *regs)
2581 {
2582     debug_stub(regs);
2583 }
2584
2585 // INT 10h Video Support Service Entry Point
2586 void VISIBLE16
2587 handle_10(struct bregs *regs)
2588 {
2589     debug_enter(regs, DEBUG_VGA_10);
2590     switch (regs->ah) {
2591     case 0x00: handle_1000(regs); break;
2592     case 0x01: handle_1001(regs); break;
2593     case 0x02: handle_1002(regs); break;
2594     case 0x03: handle_1003(regs); break;
2595     case 0x04: handle_1004(regs); break;
2596     case 0x05: handle_1005(regs); break;
2597     case 0x06: handle_1006(regs); break;
2598     case 0x07: handle_1007(regs); break;
2599     case 0x08: handle_1008(regs); break;
2600     case 0x09: handle_1009(regs); break;
2601     case 0x0a: handle_100a(regs); break;
2602     case 0x0b: handle_100b(regs); break;
2603     case 0x0c: handle_100c(regs); break;
2604     case 0x0d: handle_100d(regs); break;
2605     case 0x0e: handle_100e(regs); break;
2606     case 0x0f: handle_100f(regs); break;
2607     case 0x10: handle_1010(regs); break;
2608     case 0x11: handle_1011(regs); break;
2609     case 0x12: handle_1012(regs); break;
2610     case 0x13: handle_1013(regs); break;
2611     case 0x1a: handle_101a(regs); break;
2612     case 0x1b: handle_101b(regs); break;
2613     case 0x1c: handle_101c(regs); break;
2614     case 0x4f: handle_104f(regs); break;
2615     default:   handle_10XX(regs); break;
2616     }
2617 }
2618
2619
2620 /****************************************************************
2621  * VGA post
2622  ****************************************************************/
2623
2624 static void
2625 init_bios_area()
2626 {
2627     // init detected hardware BIOS Area
2628     // set 80x25 color (not clear from RBIL but usual)
2629     u16 eqf = GET_BDA(equipment_list_flags);
2630     SET_BDA(equipment_list_flags, (eqf & 0xffcf) | 0x20);
2631
2632     // Just for the first int10 find its children
2633
2634     // the default char height
2635     SET_BDA(char_height, 0x10);
2636
2637     // Clear the screen
2638     SET_BDA(video_ctl, 0x60);
2639
2640     // Set the basic screen we have
2641     SET_BDA(video_switches, 0xf9);
2642
2643     // Set the basic modeset options
2644     SET_BDA(modeset_ctl, 0x51);
2645
2646     // Set the  default MSR
2647     SET_BDA(video_msr, 0x09);
2648 }
2649
2650 static void
2651 init_vga_card()
2652 {
2653     // switch to color mode and enable CPU access 480 lines
2654     outb(0xc3, VGAREG_WRITE_MISC_OUTPUT);
2655     // more than 64k 3C4/04
2656     outb(0x04, VGAREG_SEQU_ADDRESS);
2657     outb(0x02, VGAREG_SEQU_DATA);
2658 }
2659
2660 void VISIBLE16
2661 vga_post(struct bregs *regs)
2662 {
2663     debug_enter(regs, DEBUG_VGA_POST);
2664
2665     init_vga_card();
2666
2667     init_bios_area();
2668
2669     if (CONFIG_VBE)
2670         vbe_init();
2671
2672     extern void entry_10(void);
2673     SET_IVT(0x10, get_global_seg(), (u32)entry_10);
2674
2675     if (CONFIG_CIRRUS)
2676         cirrus_init();
2677
2678     // XXX - clear screen and display info
2679
2680     // XXX: fill it
2681     SET_VGA(video_save_pointer_table[0], (u32)video_param_table);
2682     SET_VGA(video_save_pointer_table[1], get_global_seg());
2683
2684     // Fixup checksum
2685     extern u8 _rom_header_size, _rom_header_checksum;
2686     SET_VGA(_rom_header_checksum, 0);
2687     u8 sum = -checksum_far(get_global_seg(), 0, _rom_header_size * 512);
2688     SET_VGA(_rom_header_checksum, sum);
2689 }