usr.bin/doscmd/video.c

   1 /*
   2  * Copyright (c) 2001 The FreeBSD Project, Inc.
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY The FreeBSD Project, Inc. AND CONTRIBUTORS
  15  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  16  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  17  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL The FreeBSD Project, Inc. OR
  18  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  19  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  20  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
  21  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  22  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
  23  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
  24  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  25  *
  26  * $FreeBSD: src/usr.bin/doscmd/video.c,v 1.5.2.1 2002/04/25 11:04:51 tg Exp $
  27  * $DragonFly: src/usr.bin/doscmd/video.c,v 1.3 2003/10/04 20:36:43 hmp Exp $
  28  */
  29
  30 #include <sys/types.h>
  31 #include <sys/mman.h>
  32 #include <err.h>
  33 #include <paths.h>
  34 #include <unistd.h>
  35
  36 #include "doscmd.h"
  37 #include "AsyncIO.h"
  38 #include "tty.h"
  39 #include "video.h"
  40 #include "vparams.h"
  41
  42 /*
  43  * Global variables
  44  */
  45
  46 /* VGA registers */
  47 u_int8_t VGA_CRTC[CRTC_Size];
  48 u_int8_t VGA_ATC[ATC_Size];
  49 u_int8_t VGA_TSC[TSC_Size];
  50 u_int8_t VGA_GDC[GDC_Size];
  51
  52 /* VGA status information */
  53 u_int8_t vga_status[64];
  54
  55 /* Table of supported video modes. */
  56 vmode_t vmodelist[] = {
  57     {0x00, 0x17, TEXT, 16, 8, 2, 0xb8000, FONT8x16},
  58     {0x01, 0x17, TEXT, 16, 8, 2, 0xb8000, FONT8x16},
  59     {0x02, 0x18, TEXT, 16, 8, 2, 0xb8000, FONT8x16},
  60     {0x03, 0x18, TEXT, 16, 8, 2, 0xb8000, FONT8x16},
  61     {0x04, 0x04, GRAPHICS, 4, 1, 0, 0xb8000, FONT8x8},
  62     {0x05, 0x05, GRAPHICS, 4, 1, 0, 0xb8000, FONT8x8},
  63     {0x06, 0x06, GRAPHICS, 2, 1, 0, 0xb8000, FONT8x8},
  64     {0x07, 0x19, TEXT, 1, 8, 2, 0xb0000, FONT8x16},
  65     {0x08, 0x08, NOMODE, 0, 0, 0, 0, 0},
  66     {0x09, 0x09, NOMODE, 0, 0, 0, 0, 0},
  67     {0x0a, 0x0a, NOMODE, 0, 0, 0, 0, 0},
  68     {0x0b, 0x0b, NOMODE, 0, 0, 0, 0, 0},
  69     {0x0c, 0x0c, NOMODE, 0, 0, 0, 0, 0},
  70     {0x0d, 0x0d, GRAPHICS, 16, 8, 0, 0xa0000, FONT8x8},
  71     {0x0e, 0x0e, GRAPHICS, 16, 4, 0, 0xa0000, FONT8x8},
  72     {0x0f, 0x11, GRAPHICS, 1, 2, 1, 0xa0000, FONT8x14},
  73     {0x10, 0x12, GRAPHICS, 16, 2, 1, 0xa0000, FONT8x14},
  74     {0x11, 0x1a, GRAPHICS, 2, 1, 3, 0xa0000, FONT8x16},
  75     {0x12, 0x1b, GRAPHICS, 16, 1, 3, 0xa0000, FONT8x16},
  76     /*     {0x13, 0x1c, GRAPHICS, 256, 1, 0, 0xa0000, FONT8x8}, */
  77 };
  78
  79 #define NUMMODES        (sizeof(vmodelist) / sizeof(vmode_t))
  80
  81 /*
  82  * Local functions
  83  */
  84 static void     init_vga(void);
  85 static u_int8_t video_inb(int);
  86 static void     video_outb(int, u_int8_t);
  87
  88 /*
  89  * Local types and variables
  90  */
  91
  92 /* Save Table and assorted variables */
  93 struct VideoSaveTable {
  94     u_short     video_parameter_table[2];
  95     u_short     parameter_dynamic_save_area[2];         /* Not used */
  96     u_short     alphanumeric_character_set_override[2]; /* Not used */
  97     u_short     graphics_character_set_override[2];     /* Not used */
  98     u_short     secondary_save_table[2];        /* Not used */
  99     u_short     mbz[4];
 100 };
 101
 102 struct SecondaryVideoSaveTable {
 103     u_short     length;
 104     u_short     display_combination_code_table[2];
 105     u_short     alphanumeric_character_set_override[2]; /* Not used */
 106     u_short     user_palette_profile_table[2];          /* Not used */
 107     u_short     mbz[6];
 108 };
 109
 110 struct VideoSaveTable *vsp;
 111 struct SecondaryVideoSaveTable *svsp;
 112
 113 /*
 114  * Read and write the VGA port
 115  */
 116
 117 /* Save the selected index register */
 118 static u_int8_t crtc_index, atc_index, tsc_index, gdc_index;
 119 /* Toggle between index and data on port ATC_WritePort */
 120 static u_int8_t set_atc_index = 1;
 121
 122 static u_int8_t
 123 video_inb(int port)
 124 {
 125     switch(port) {
 126     case CRTC_DataPortColor:
 127         return VGA_CRTC[crtc_index];
 128     case CRTC_IndexPortColor:
 129         return crtc_index;
 130     case ATC_ReadPort:
 131         return VGA_ATC[atc_index];
 132     case TSC_DataPort:
 133         return VGA_TSC[tsc_index];
 134     case TSC_IndexPort:
 135         return tsc_index;
 136     case GDC_DataPort:
 137         return VGA_GDC[gdc_index];
 138     case GDC_IndexPort:
 139         return gdc_index;
 140     case VGA_InputStatus1Port:
 141         set_atc_index = 1;
 142         VGA_InputStatus1 = (VGA_InputStatus1 + 1) & 15;
 143         return VGA_InputStatus1;
 144     default:
 145         return 0;
 146     }
 147 }
 148
 149 static void
 150 video_outb(int port, u_int8_t value)
 151 {
 152 /* XXX */
 153 #define row     (CursRow0)
 154 #define col     (CursCol0)
 155
 156     int cp;
 157
 158     switch (port) {
 159     case CRTC_IndexPortColor:
 160         crtc_index = value;
 161         break;
 162     case CRTC_DataPortColor:
 163         VGA_CRTC[crtc_index] = value;
 164         switch (crtc_index) {
 165         case CRTC_CurLocHi:     /* Update cursor position in BIOS */
 166             cp = row * DpyCols + col;
 167             cp &= 0xff;
 168             cp |= value << 8;
 169             row = cp / DpyCols;
 170             col = cp % DpyCols;
 171             break;
 172         case CRTC_CurLocLo:     /* Update cursor position in BIOS */
 173             cp = row * DpyCols + col;
 174             cp &= 0xff00;
 175             cp |= value;
 176             row = cp / DpyCols;
 177             col = cp % DpyCols;
 178             break;
 179         default:
 180             debug(D_VIDEO, "VGA: outb 0x%04x, 0x%02x at index 0x%02x\n",
 181                   port, value, crtc_index);
 182             break;
 183         }
 184     case CRTC_IndexPortMono:    /* Not used */
 185         break;
 186     case CRTC_DataPortMono:     /* Not used */
 187         break;
 188     case ATC_WritePort:
 189         if (set_atc_index)
 190             atc_index = value;
 191         else {
 192             VGA_ATC[atc_index] = value;
 193             switch (atc_index) {
 194             default:
 195                 debug(D_VIDEO, "VGA: outb 0x%04x, 0x%02x at index 0x%02x\n",
 196                       port, value, crtc_index);
 197                 break;
 198             }
 199         }
 200         set_atc_index = 1 - set_atc_index;
 201         break;
 202     case TSC_IndexPort:
 203         tsc_index = value;
 204         break;
 205     case TSC_DataPort:
 206         VGA_TSC[tsc_index] = value;
 207         switch (tsc_index) {
 208         default:
 209             debug(D_VIDEO, "VGA: outb 0x%04x, 0x%02x at index 0x%02x\n",
 210                   port, value, crtc_index);
 211             break;
 212         }
 213         break;
 214     case GDC_IndexPort:
 215         gdc_index = value;
 216         break;
 217     case GDC_DataPort:
 218         VGA_GDC[gdc_index] = value;
 219 #if 0
 220         switch (gdc_index) {
 221         default:
 222             debug(D_VIDEO, "VGA: outb 0x%04x, 0x%02x at index 0x%02x\n",
 223                   port, value, crtc_index);
 224
 225             break;
 226         }
 227 #endif
 228         break;
 229     default:
 230         debug(D_ALWAYS, "VGA: Unknown port 0x%4x\n", port);
 231         break;
 232     }
 233
 234     return;
 235 #undef row
 236 #undef col
 237 }
 238
 239 void
 240 video_init(void)
 241 {
 242     /* If we are running under X, get a connection to the X server and create
 243        an empty window of size (1, 1). It makes a couple of init functions a
 244        lot easier. */
 245     if (xmode) {
 246         init_window();
 247
 248         /* Set VGA emulator to a sane state */
 249         init_vga();
 250
 251         /* Initialize mode 3 (text, 80x25, 16 colors) */
 252         init_mode(3);
 253     }
 254
 255     /* Define all known I/O port handlers */
 256     if (!raw_kbd) {
 257         define_input_port_handler(CRTC_IndexPortColor, video_inb);
 258         define_input_port_handler(CRTC_DataPortColor, video_inb);
 259         define_input_port_handler(ATC_ReadPort, video_inb);
 260         define_input_port_handler(TSC_IndexPort, video_inb);
 261         define_input_port_handler(TSC_DataPort, video_inb);
 262         define_input_port_handler(GDC_IndexPort, video_inb);
 263         define_input_port_handler(GDC_DataPort, video_inb);
 264         define_input_port_handler(VGA_InputStatus1Port, video_inb);
 265
 266         define_output_port_handler(CRTC_IndexPortColor, video_outb);
 267         define_output_port_handler(CRTC_DataPortColor, video_outb);
 268         define_output_port_handler(ATC_WritePort, video_outb);
 269         define_output_port_handler(TSC_IndexPort, video_outb);
 270         define_output_port_handler(TSC_DataPort, video_outb);
 271         define_output_port_handler(GDC_IndexPort, video_outb);
 272         define_output_port_handler(GDC_DataPort, video_outb);
 273     }
 274
 275     redirect0 = isatty(0) == 0 || !xmode ;
 276     redirect1 = isatty(1) == 0 || !xmode ;
 277     redirect2 = isatty(2) == 0 || !xmode ;
 278
 279     return;
 280 }
 281
 282 void
 283 video_bios_init(void)
 284 {
 285     u_char *p;
 286     u_long vec;
 287
 288     if (raw_kbd)
 289         return;
 290
 291     /*
 292      * Put the Video Save Table Pointer @ C000:0000
 293      * Put the Secondary Video Save Table Pointer @ C000:0020
 294      * Put the Display Combination Code table @ C000:0040
 295      * Put the Video Parameter table @ C000:1000 - C000:2FFF
 296      */
 297     BIOS_SaveTablePointer = 0xC0000000;
 298
 299     vsp = (struct VideoSaveTable *)0xC0000L;
 300     memset(vsp, 0, sizeof(struct VideoSaveTable));
 301     svsp = (struct SecondaryVideoSaveTable *)0xC0020L;
 302
 303     vsp->video_parameter_table[0] = 0x1000;
 304     vsp->video_parameter_table[1] = 0xC000;
 305
 306     vsp->secondary_save_table[0] = 0x0020;
 307     vsp->secondary_save_table[1] = 0xC000;
 308
 309     svsp->display_combination_code_table[0] = 0x0040;
 310     svsp->display_combination_code_table[1] = 0xC000;
 311
 312     p = (u_char *)0xC0040;
 313     *p++ = 2;           /* Only support 2 combinations currently */
 314     *p++ = 1;           /* Version # */
 315     *p++ = 8;           /* We won't use more than type 8 */
 316     *p++ = 0;           /* Reserved */
 317     *p++ = 0; *p++ = 0; /* No Display No Display */
 318     *p++ = 0; *p++ = 8; /* No Display VGA Color */
 319
 320     memcpy((void *)0xC1000, videoparams, sizeof(videoparams));
 321     ivec[0x1d] = 0xC0001000L;   /* Video Parameter Table */
 322
 323     ivec[0x42] = ivec[0x10];    /* Copy of video interrupt */
 324
 325     /* Put the current font at C000:3000; the pixels are copied in
 326        'tty.c:load_font()'. */
 327     ivec[0x1f] = 0xC0003000L;
 328     ivec[0x43] = 0xC0003000L;
 329
 330     BIOSDATA[0x8a] = 1;        /* Index into DCC table */
 331
 332     vec = insert_softint_trampoline();
 333     ivec[0x10] = vec;
 334     register_callback(vec, int10, "int 10");
 335 }
 336
 337 /* Initialize the VGA emulator
 338
 339    XXX This is not nearly finished right now.
 340 */
 341 static void
 342 init_vga(void)
 343 {
 344     int i;
 345
 346     /* Zero-fill 'dac_rgb' on allocation; the default (EGA) table has only
 347        64 entries. */
 348     dac_rgb = (struct dac_colors *)calloc(256, sizeof(struct dac_colors));
 349     if (dac_rgb == NULL)
 350         err(1, "Get memory for dac_rgb");
 351
 352     /* Copy the default DAC table to a working copy we can trash. */
 353     for (i = 0; i < 64; i++)
 354         dac_rgb[i] = dac_default64[i]; /* Structure copy */
 355
 356     /* Point 'palette[]' to the Attribute Controller space. We will only use
 357        the first 16 slots. */
 358     palette = VGA_ATC;
 359
 360     /* Get memory for the video RAM and adjust the plane pointers. */
 361     vram = calloc(256 * 1024, 1);       /* XXX */
 362     if (vram == NULL)
 363         warn("Could not get video memory; graphics modes not available.");
 364
 365     /* XXX There is probably a more efficient memory layout... */
 366     vplane0 = vram;
 367     vplane1 = vram + 0x10000;
 368     vplane2 = vram + 0x20000;
 369     vplane3 = vram + 0x30000;
 370
 371     VGA_InputStatus1 = 0;
 372 }
 373
 374 /*
 375  * Initialize the requested video mode.
 376  */
 377
 378 /* Indices into the video parameter table. We will use that array to
 379    initialize the registers on startup and when the video mode changes. */
 380 #define CRTC_Ofs        10
 381 #define ATC_Ofs         35
 382 #define TSC_Ofs         5
 383 #define GDC_Ofs         55
 384 #define MiscOutput_Ofs  9
 385
 386 void
 387 init_mode(int mode)
 388 {
 389     vmode_t vmode;
 390     int idx;                    /* Index into vmode */
 391     int pidx;                   /* Index into videoparams */
 392
 393     debug(D_VIDEO, "VGA: Set video mode to 0x%02x\n", mode);
 394
 395     idx = find_vmode(mode & 0x7f);
 396     if (idx == -1 || vmodelist[idx].type == NOMODE)
 397         err(1, "Mode 0x%02x is not supported", mode);
 398     vmode = vmodelist[idx];
 399     pidx = vmode.paramindex;
 400
 401     /* Preset VGA registers. */
 402     memcpy(VGA_CRTC, (u_int8_t *)&videoparams[pidx][CRTC_Ofs],
 403            sizeof(VGA_CRTC));
 404     memcpy(VGA_ATC, (u_int8_t *)&videoparams[pidx][ATC_Ofs],
 405            sizeof(VGA_ATC));
 406     /* Warning: the video parameter table does not contain the Sequencer's
 407        Reset register. Its default value is 0x03.*/
 408     VGA_TSC[TSC_Reset] = 0x03;
 409     memcpy(VGA_TSC + 1, (u_int8_t *)&videoparams[pidx][TSC_Ofs],
 410            sizeof(VGA_TSC) - 1);
 411     memcpy(VGA_GDC, (u_int8_t *)&videoparams[pidx][GDC_Ofs],
 412            sizeof(VGA_GDC));
 413     VGA_MiscOutput = videoparams[pidx][MiscOutput_Ofs];
 414
 415     /* Paranoia */
 416     if ((VGA_ATC[ATC_ModeCtrl] & 1) == 1 && vmode.type == TEXT)
 417         err(1, "Text mode requested, but ATC switched to graphics mode!");
 418     if ((VGA_ATC[ATC_ModeCtrl] & 1) == 0 && vmode.type == GRAPHICS)
 419         err(1, "Graphics mode requested, but ATC switched to text mode!");
 420
 421     VideoMode = mode & 0x7f;
 422     DpyCols = (u_int16_t)videoparams[pidx][0];
 423     DpyPageSize = *(u_int16_t *)&videoparams[pidx][3];
 424     ActivePageOfs = 0;
 425     CursCol0 = 0;
 426     CursRow0 = 0;
 427     CursCol1 = 0;
 428     CursRow1 = 0;
 429     CursCol2 = 0;
 430     CursRow2 = 0;
 431     CursCol3 = 0;
 432     CursRow3 = 0;
 433     CursCol4 = 0;
 434     CursRow4 = 0;
 435     CursCol5 = 0;
 436     CursRow5 = 0;
 437     CursCol6 = 0;
 438     CursRow6 = 0;
 439     CursCol7 = 0;
 440     CursRow7 = 0;
 441     CursStart = VGA_CRTC[CRTC_CursStart];
 442     CursEnd = VGA_CRTC[CRTC_CursEnd];
 443     ActivePage = 0;
 444     DpyRows = videoparams[pidx][1];
 445     CharHeight = videoparams[pidx][2];
 446
 447     CRTCPort = vmode.numcolors > 1 ? CRTC_IndexPortColor : CRTC_IndexPortMono;
 448     NumColors = vmode.numcolors;
 449     NumPages = vmode.numpages;
 450     VertResolution = vmode.vrescode;
 451     vmem = (u_int16_t *)vmode.vmemaddr;
 452
 453     /* Copy VGA related BIOS variables from 'vga_status'. */
 454     memcpy(&BIOS_VideoMode, &VideoMode, 33);
 455     BIOS_DpyRows = DpyRows;
 456     BIOS_CharHeight = CharHeight;
 457
 458     /* Load 'pixels[]' from default DAC values. */
 459     update_pixels();
 460
 461     /* Update font. */
 462     xfont = vmode.fontname;
 463     load_font();
 464
 465     /* Resize window if necessary. */
 466     resize_window();
 467
 468     /* Mmap video memory for the graphics modes. Write access to 0xa0000 -
 469        0xaffff will generate a T_PAGEFAULT trap in VM86 mode (aside: why not a
 470        SIGSEGV?), which is handled in 'trap.c:sigbus()'. */
 471     if (vmode.type == GRAPHICS) {
 472         vmem = mmap((void *)0xa0000, 64 * 1024, PROT_NONE,
 473                     MAP_ANON | MAP_FIXED | MAP_SHARED, -1, 0);
 474         if (vmem == NULL)
 475             fatal("Could not mmap() video memory");
 476
 477         /* Create an XImage to display the graphics screen. */
 478         get_ximage();
 479     } else {
 480         int i;
 481
 482         get_lines();
 483         if (mode & 0x80)
 484             return;
 485         /* Initialize video memory with black background, white foreground */
 486         vattr = 0x0700;
 487         for (i = 0; i < DpyPageSize / 2; ++i)
 488             vmem[i] = vattr;
 489     }
 490
 491     return;
 492 }
 493
 494 /* Find the requested mode in the 'vmodelist' table. This function returns the
 495    index into this table; we will also use the index for accessing the
 496    'videoparams' array. */
 497 int find_vmode(int mode)
 498 {
 499     unsigned i;
 500
 501     for (i = 0; i < NUMMODES; i++)
 502         if (vmodelist[i].modenumber == mode)
 503             return i;
 504
 505     return -1;
 506 }
 507
 508 /* Handle access to the graphics memory.
 509
 510    Simply changing the protection for the memory is not enough, unfortunately.
 511    It would only work for the 256 color modes, where a memory byte contains
 512    the color value of one pixel. The 16 color modes (and 4 color modes) make
 513    use of four bit planes which overlay the first 64K of video memory. The
 514    bits are distributed into these bit planes according to the GDC state, so
 515    we will have to emulate the CPU instructions (see 'cpu.c:emu_instr()').
 516
 517    Handling the 256 color modes will be a bit easier, once we support those at
 518    all. */
 519 int
 520 vmem_pageflt(struct sigframe *sf)
 521 {
 522     regcontext_t *REGS = (regcontext_t *)(&sf->sf_uc.uc_mcontext);
 523
 524     /* The ATC's Mode Control register tells us whether 4 or 8 color bits are
 525        used */
 526     if (VGA_ATC[ATC_ModeCtrl] & (1 << 6)) {
 527         /* 256 colors, allow writes; the protection will be set back to
 528            PROT_READ at the next display update */
 529         mprotect(vmem, 64 * 1024, PROT_READ | PROT_WRITE);
 530         return 0;
 531     }
 532
 533     /* There's no need to change the protection in the 16 color modes, we will
 534        write to 'vram'. Just emulate the next instruction. */
 535     return emu_instr(REGS);
 536 }
 537
 538 /* We need to keep track of the latches' contents.*/
 539 static u_int8_t latch0, latch1, latch2, latch3;
 540
 541 /* Read a byte from the video memory. 'vga_read()' is called from
 542    'cpu.c:read_byte()' and will emulate the VGA read modes. */
 543 u_int8_t
 544 vga_read(u_int32_t addr)
 545 {
 546     u_int32_t dst;
 547
 548     /* 'addr' lies between 0xa0000 and 0xaffff. */
 549     dst = addr - 0xa0000;
 550
 551     /* Fill latches. */
 552     latch0 = vplane0[dst];
 553     latch1 = vplane1[dst];
 554     latch2 = vplane2[dst];
 555     latch3 = vplane3[dst];
 556
 557     /* Select read mode. */
 558     if ((VGA_GDC[GDC_Mode] & 0x80) == 0)
 559         /* Read Mode 0; return the byte from the selected bit plane. */
 560         return vram[dst + (VGA_GDC[GDC_ReadMapSelect] & 3) * 0x10000];
 561
 562     /* Read Mode 1 */
 563     debug(D_ALWAYS, "VGA: Read Mode 1 not implemented\n");
 564     return 0;
 565 }
 566
 567 /* Write a byte to the video memory. 'vga_write()' is called from
 568    'cpu.c:write_word()' and will emulate the VGA write modes. Not all four
 569    modes are implemented yet, nor are the addressing modes (odd/even, chain4).
 570    (NB: I think the latter will have to be done in 'tty_graphics_update()').
 571    */
 572 void
 573 vga_write(u_int32_t addr, u_int8_t val)
 574 {
 575     u_int32_t dst;
 576     u_int8_t c0, c1, c2, c3;
 577     u_int8_t m0, m1, m2, m3;
 578     u_int8_t mask;
 579
 580 #if 0
 581     unsigned i;
 582
 583     debug(D_VIDEO, "VGA: Write 0x%02x to 0x%x\n", val, addr);
 584     debug(D_VIDEO, "   GDC: ");
 585     for (i = 0; i < sizeof(VGA_GDC); i++)
 586         debug(D_VIDEO, "%02x ", VGA_GDC[i]);
 587     debug(D_VIDEO, "\n");
 588     debug(D_VIDEO, "   TSC: ");
 589     for (i = 0; i < sizeof(VGA_TSC); i++)
 590         debug(D_VIDEO, "%02x ", VGA_TSC[i]);
 591     debug(D_VIDEO, "\n");
 592 #endif
 593
 594     /* 'addr' lies between 0xa0000 and 0xaffff. */
 595     dst = addr - 0xa0000;
 596
 597     c0 = latch0;
 598     c1 = latch1;
 599     c2 = latch2;
 600     c3 = latch3;
 601
 602     /* Select write mode. */
 603     switch (VGA_GDC[GDC_Mode] & 3) {
 604     case 0:
 605         mask = VGA_GDC[GDC_BitMask];
 606
 607         if (VGA_GDC[GDC_DataRotate] & 7)
 608             debug(D_ALWAYS, "VGA: Data Rotate != 0\n");
 609
 610         /* Select function.  */
 611         switch (VGA_GDC[GDC_DataRotate] & 0x18) {
 612         case 0x00:              /* replace */
 613             m0 = VGA_GDC[GDC_SetReset] & 1 ? mask : 0x00;
 614             m1 = VGA_GDC[GDC_SetReset] & 2 ? mask : 0x00;
 615             m2 = VGA_GDC[GDC_SetReset] & 4 ? mask : 0x00;
 616             m3 = VGA_GDC[GDC_SetReset] & 8 ? mask : 0x00;
 617
 618             c0 = VGA_GDC[GDC_EnableSetReset] & 1 ? c0 & ~mask : val & ~mask;
 619             c1 = VGA_GDC[GDC_EnableSetReset] & 2 ? c1 & ~mask : val & ~mask;
 620             c2 = VGA_GDC[GDC_EnableSetReset] & 4 ? c2 & ~mask : val & ~mask;
 621             c3 = VGA_GDC[GDC_EnableSetReset] & 8 ? c3 & ~mask : val & ~mask;
 622
 623             c0 |= m0;
 624             c1 |= m1;
 625             c2 |= m2;
 626             c3 |= m3;
 627             break;
 628         case 0x08:              /* AND */
 629             m0 = VGA_GDC[GDC_SetReset] & 1 ? 0xff : ~mask;
 630             m1 = VGA_GDC[GDC_SetReset] & 2 ? 0xff : ~mask;
 631             m2 = VGA_GDC[GDC_SetReset] & 4 ? 0xff : ~mask;
 632             m3 = VGA_GDC[GDC_SetReset] & 8 ? 0xff : ~mask;
 633
 634             c0 = VGA_GDC[GDC_EnableSetReset] & 1 ? c0 & m0 : val & m0;
 635             c1 = VGA_GDC[GDC_EnableSetReset] & 2 ? c1 & m1 : val & m1;
 636             c2 = VGA_GDC[GDC_EnableSetReset] & 4 ? c2 & m2 : val & m2;
 637             c3 = VGA_GDC[GDC_EnableSetReset] & 8 ? c3 & m3 : val & m3;
 638             break;
 639         case 0x10:              /* OR */
 640             m0 = VGA_GDC[GDC_SetReset] & 1 ? mask : 0x00;
 641             m1 = VGA_GDC[GDC_SetReset] & 2 ? mask : 0x00;
 642             m2 = VGA_GDC[GDC_SetReset] & 4 ? mask : 0x00;
 643             m3 = VGA_GDC[GDC_SetReset] & 8 ? mask : 0x00;
 644
 645             c0 = VGA_GDC[GDC_EnableSetReset] & 1 ? c0 | m0 : val | m0;
 646             c1 = VGA_GDC[GDC_EnableSetReset] & 2 ? c1 | m1 : val | m1;
 647             c2 = VGA_GDC[GDC_EnableSetReset] & 4 ? c2 | m2 : val | m2;
 648             c3 = VGA_GDC[GDC_EnableSetReset] & 8 ? c3 | m3 : val | m3;
 649             break;
 650         case 0x18:              /* XOR */
 651             m0 = VGA_GDC[GDC_SetReset] & 1 ? mask : 0x00;
 652             m1 = VGA_GDC[GDC_SetReset] & 2 ? mask : 0x00;
 653             m2 = VGA_GDC[GDC_SetReset] & 4 ? mask : 0x00;
 654             m3 = VGA_GDC[GDC_SetReset] & 8 ? mask : 0x00;
 655
 656             c0 = VGA_GDC[GDC_EnableSetReset] & 1 ? c0 ^ m0 : val ^ m0;
 657             c1 = VGA_GDC[GDC_EnableSetReset] & 2 ? c1 ^ m1 : val ^ m1;
 658             c2 = VGA_GDC[GDC_EnableSetReset] & 4 ? c2 ^ m2 : val ^ m2;
 659             c3 = VGA_GDC[GDC_EnableSetReset] & 8 ? c3 ^ m3 : val ^ m3;
 660             break;
 661         }
 662         break;
 663     case 1:
 664         /* Just copy the latches' content to the desired destination
 665            address. */
 666         break;
 667     case 2:
 668         mask = VGA_GDC[GDC_BitMask];
 669
 670         /* select function */
 671         switch (VGA_GDC[GDC_DataRotate] & 0x18) {
 672         case 0x00:              /* replace */
 673             m0 = (val & 1 ? 0xff : 0x00) & mask;
 674             m1 = (val & 2 ? 0xff : 0x00) & mask;
 675             m2 = (val & 4 ? 0xff : 0x00) & mask;
 676             m3 = (val & 8 ? 0xff : 0x00) & mask;
 677
 678             c0 &= ~mask;
 679             c1 &= ~mask;
 680             c2 &= ~mask;
 681             c3 &= ~mask;
 682
 683             c0 |= m0;
 684             c1 |= m1;
 685             c2 |= m2;
 686             c3 |= m3;
 687             break;
 688         case 0x08:              /* AND */
 689             m0 = (val & 1 ? 0xff : 0x00) | ~mask;
 690             m1 = (val & 2 ? 0xff : 0x00) | ~mask;
 691             m2 = (val & 4 ? 0xff : 0x00) | ~mask;
 692             m3 = (val & 8 ? 0xff : 0x00) | ~mask;
 693
 694             c0 &= m0;
 695             c1 &= m1;
 696             c2 &= m2;
 697             c3 &= m3;
 698             break;
 699         case 0x10:              /* OR */
 700             m0 = (val & 1 ? 0xff : 0x00) & mask;
 701             m1 = (val & 2 ? 0xff : 0x00) & mask;
 702             m2 = (val & 4 ? 0xff : 0x00) & mask;
 703             m3 = (val & 8 ? 0xff : 0x00) & mask;
 704
 705             c0 |= m0;
 706             c1 |= m1;
 707             c2 |= m2;
 708             c3 |= m3;
 709             break;
 710         case 0x18:              /* XOR */
 711             m0 = (val & 1 ? 0xff : 0x00) & mask;
 712             m1 = (val & 2 ? 0xff : 0x00) & mask;
 713             m2 = (val & 4 ? 0xff : 0x00) & mask;
 714             m3 = (val & 8 ? 0xff : 0x00) & mask;
 715
 716             c0 ^= m0;
 717             c1 ^= m1;
 718             c2 ^= m2;
 719             c3 ^= m3;
 720             break;
 721         }
 722         break;
 723     case 3:
 724         /* not yet */
 725         debug(D_ALWAYS, "VGA: Write Mode 3 not implemented\n");
 726         break;
 727     }
 728
 729     /* Write back changed byte, depending on Map Mask register. */
 730     if (VGA_TSC[TSC_MapMask] & 1)
 731         vplane0[dst] = c0;
 732     if (VGA_TSC[TSC_MapMask] & 2)
 733         vplane1[dst] = c1;
 734     if (VGA_TSC[TSC_MapMask] & 4)
 735         vplane2[dst] = c2;
 736     if (VGA_TSC[TSC_MapMask] & 8)
 737         vplane3[dst] = c3;
 738
 739     return;
 740 }