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