drivers/video/fbdev/vga16fb.c

   1 /*
   2  * linux/drivers/video/vga16.c -- VGA 16-color framebuffer driver
   3  *
   4  * Copyright 1999 Ben Pfaff <[email protected]> and Petr Vandrovec <[email protected]>
   5  * Based on VGA info at http://www.goodnet.com/~tinara/FreeVGA/home.htm
   6  * Based on VESA framebuffer (c) 1998 Gerd Knorr <[email protected]>
   7  *
   8  * This file is subject to the terms and conditions of the GNU General
   9  * Public License.  See the file COPYING in the main directory of this
  10  * archive for more details.
  11  */
  12
  13 #include <linux/aperture.h>
  14 #include <linux/module.h>
  15 #include <linux/kernel.h>
  16 #include <linux/errno.h>
  17 #include <linux/string.h>
  18 #include <linux/mm.h>
  19 #include <linux/delay.h>
  20 #include <linux/fb.h>
  21 #include <linux/ioport.h>
  22 #include <linux/init.h>
  23 #include <linux/platform_device.h>
  24 #include <linux/screen_info.h>
  25
  26 #include <asm/io.h>
  27 #include <video/vga.h>
  28
  29 #define MODE_SKIP4      1
  30 #define MODE_8BPP       2
  31 #define MODE_CFB        4
  32 #define MODE_TEXT       8
  33
  34 /* --------------------------------------------------------------------- */
  35
  36 /*
  37  * card parameters
  38  */
  39
  40 struct vga16fb_par {
  41         /* structure holding original VGA register settings when the
  42            screen is blanked */
  43         struct {
  44                 unsigned char   SeqCtrlIndex;     /* Sequencer Index reg.   */
  45                 unsigned char   CrtCtrlIndex;     /* CRT-Contr. Index reg.  */
  46                 unsigned char   CrtMiscIO;        /* Miscellaneous register */
  47                 unsigned char   HorizontalTotal;  /* CRT-Controller:00h */
  48                 unsigned char   HorizDisplayEnd;  /* CRT-Controller:01h */
  49                 unsigned char   StartHorizRetrace;/* CRT-Controller:04h */
  50                 unsigned char   EndHorizRetrace;  /* CRT-Controller:05h */
  51                 unsigned char   Overflow;         /* CRT-Controller:07h */
  52                 unsigned char   StartVertRetrace; /* CRT-Controller:10h */
  53                 unsigned char   EndVertRetrace;   /* CRT-Controller:11h */
  54                 unsigned char   ModeControl;      /* CRT-Controller:17h */
  55                 unsigned char   ClockingMode;     /* Seq-Controller:01h */
  56         } vga_state;
  57         struct vgastate state;
  58         unsigned int ref_count;
  59         int palette_blanked, vesa_blanked, mode, isVGA;
  60         u8 misc, pel_msk, vss, clkdiv;
  61         u8 crtc[VGA_CRT_C];
  62 };
  63
  64 /* --------------------------------------------------------------------- */
  65
  66 static struct fb_var_screeninfo vga16fb_defined = {
  67         .xres           = 640,
  68         .yres           = 480,
  69         .xres_virtual   = 640,
  70         .yres_virtual   = 480,
  71         .bits_per_pixel = 4,
  72         .activate       = FB_ACTIVATE_TEST,
  73         .height         = -1,
  74         .width          = -1,
  75         .pixclock       = 39721,
  76         .left_margin    = 48,
  77         .right_margin   = 16,
  78         .upper_margin   = 33,
  79         .lower_margin   = 10,
  80         .hsync_len      = 96,
  81         .vsync_len      = 2,
  82         .vmode          = FB_VMODE_NONINTERLACED,
  83 };
  84
  85 /* name should not depend on EGA/VGA */
  86 static const struct fb_fix_screeninfo vga16fb_fix = {
  87         .id             = "VGA16 VGA",
  88         .smem_start     = VGA_FB_PHYS_BASE,
  89         .smem_len       = VGA_FB_PHYS_SIZE,
  90         .type           = FB_TYPE_VGA_PLANES,
  91         .type_aux       = FB_AUX_VGA_PLANES_VGA4,
  92         .visual         = FB_VISUAL_PSEUDOCOLOR,
  93         .xpanstep       = 8,
  94         .ypanstep       = 1,
  95         .line_length    = 640 / 8,
  96         .accel          = FB_ACCEL_NONE
  97 };
  98
  99 /* The VGA's weird architecture often requires that we read a byte and
 100    write a byte to the same location.  It doesn't matter *what* byte
 101    we write, however.  This is because all the action goes on behind
 102    the scenes in the VGA's 32-bit latch register, and reading and writing
 103    video memory just invokes latch behavior.
 104
 105    To avoid race conditions (is this necessary?), reading and writing
 106    the memory byte should be done with a single instruction.  One
 107    suitable instruction is the x86 bitwise OR.  The following
 108    read-modify-write routine should optimize to one such bitwise
 109    OR. */
 110 static inline void rmw(volatile char __iomem *p)
 111 {
 112         readb(p);
 113         writeb(1, p);
 114 }
 115
 116 /* Set the Graphics Mode Register, and return its previous value.
 117    Bits 0-1 are write mode, bit 3 is read mode. */
 118 static inline int setmode(int mode)
 119 {
 120         int oldmode;
 121
 122         oldmode = vga_io_rgfx(VGA_GFX_MODE);
 123         vga_io_w(VGA_GFX_D, mode);
 124         return oldmode;
 125 }
 126
 127 /* Select the Bit Mask Register and return its value. */
 128 static inline int selectmask(void)
 129 {
 130         return vga_io_rgfx(VGA_GFX_BIT_MASK);
 131 }
 132
 133 /* Set the value of the Bit Mask Register.  It must already have been
 134    selected with selectmask(). */
 135 static inline void setmask(int mask)
 136 {
 137         vga_io_w(VGA_GFX_D, mask);
 138 }
 139
 140 /* Set the Data Rotate Register and return its old value.
 141    Bits 0-2 are rotate count, bits 3-4 are logical operation
 142    (0=NOP, 1=AND, 2=OR, 3=XOR). */
 143 static inline int setop(int op)
 144 {
 145         int oldop;
 146
 147         oldop = vga_io_rgfx(VGA_GFX_DATA_ROTATE);
 148         vga_io_w(VGA_GFX_D, op);
 149         return oldop;
 150 }
 151
 152 /* Set the Enable Set/Reset Register and return its old value.
 153    The code here always uses value 0xf for this register. */
 154 static inline int setsr(int sr)
 155 {
 156         int oldsr;
 157
 158         oldsr = vga_io_rgfx(VGA_GFX_SR_ENABLE);
 159         vga_io_w(VGA_GFX_D, sr);
 160         return oldsr;
 161 }
 162
 163 /* Set the Set/Reset Register and return its old value. */
 164 static inline int setcolor(int color)
 165 {
 166         int oldcolor;
 167
 168         oldcolor = vga_io_rgfx(VGA_GFX_SR_VALUE);
 169         vga_io_w(VGA_GFX_D, color);
 170         return oldcolor;
 171 }
 172
 173 /* Return the value in the Graphics Address Register. */
 174 static inline int getindex(void)
 175 {
 176         return vga_io_r(VGA_GFX_I);
 177 }
 178
 179 /* Set the value in the Graphics Address Register. */
 180 static inline void setindex(int index)
 181 {
 182         vga_io_w(VGA_GFX_I, index);
 183 }
 184
 185 /* Check if the video mode is supported by the driver */
 186 static inline int check_mode_supported(const struct screen_info *si)
 187 {
 188         /* non-x86 architectures treat orig_video_isVGA as a boolean flag */
 189 #if defined(CONFIG_X86)
 190         /* only EGA and VGA in 16 color graphic mode are supported */
 191         if (si->orig_video_isVGA != VIDEO_TYPE_EGAC &&
 192             si->orig_video_isVGA != VIDEO_TYPE_VGAC)
 193                 return -ENODEV;
 194
 195         if (si->orig_video_mode != 0x0D &&      /* 320x200/4 (EGA) */
 196             si->orig_video_mode != 0x0E &&      /* 640x200/4 (EGA) */
 197             si->orig_video_mode != 0x10 &&      /* 640x350/4 (EGA) */
 198             si->orig_video_mode != 0x12)        /* 640x480/4 (VGA) */
 199                 return -ENODEV;
 200 #endif
 201         return 0;
 202 }
 203
 204 static void vga16fb_pan_var(struct fb_info *info,
 205                             struct fb_var_screeninfo *var)
 206 {
 207         struct vga16fb_par *par = info->par;
 208         u32 xoffset, pos;
 209
 210         xoffset = var->xoffset;
 211         if (info->var.bits_per_pixel == 8) {
 212                 pos = (info->var.xres_virtual * var->yoffset + xoffset) >> 2;
 213         } else if (par->mode & MODE_TEXT) {
 214                 int fh = 16; // FIXME !!! font height. Fugde for now.
 215                 pos = (info->var.xres_virtual * (var->yoffset / fh) + xoffset) >> 3;
 216         } else {
 217                 if (info->var.nonstd)
 218                         xoffset--;
 219                 pos = (info->var.xres_virtual * var->yoffset + xoffset) >> 3;
 220         }
 221         vga_io_wcrt(VGA_CRTC_START_HI, pos >> 8);
 222         vga_io_wcrt(VGA_CRTC_START_LO, pos & 0xFF);
 223         /* if we support CFB4, then we must! support xoffset with pixel
 224          * granularity if someone supports xoffset in bit resolution */
 225         vga_io_r(VGA_IS1_RC);           /* reset flip-flop */
 226         vga_io_w(VGA_ATT_IW, VGA_ATC_PEL);
 227         if (info->var.bits_per_pixel == 8)
 228                 vga_io_w(VGA_ATT_IW, (xoffset & 3) << 1);
 229         else
 230                 vga_io_w(VGA_ATT_IW, xoffset & 7);
 231         vga_io_r(VGA_IS1_RC);
 232         vga_io_w(VGA_ATT_IW, 0x20);
 233 }
 234
 235 static void vga16fb_update_fix(struct fb_info *info)
 236 {
 237         if (info->var.bits_per_pixel == 4) {
 238                 if (info->var.nonstd) {
 239                         info->fix.type = FB_TYPE_PACKED_PIXELS;
 240                         info->fix.line_length = info->var.xres_virtual / 2;
 241                 } else {
 242                         info->fix.type = FB_TYPE_VGA_PLANES;
 243                         info->fix.type_aux = FB_AUX_VGA_PLANES_VGA4;
 244                         info->fix.line_length = info->var.xres_virtual / 8;
 245                 }
 246         } else if (info->var.bits_per_pixel == 0) {
 247                 info->fix.type = FB_TYPE_TEXT;
 248                 info->fix.type_aux = FB_AUX_TEXT_CGA;
 249                 info->fix.line_length = info->var.xres_virtual / 4;
 250         } else {        /* 8bpp */
 251                 if (info->var.nonstd) {
 252                         info->fix.type = FB_TYPE_VGA_PLANES;
 253                         info->fix.type_aux = FB_AUX_VGA_PLANES_CFB8;
 254                         info->fix.line_length = info->var.xres_virtual / 4;
 255                 } else {
 256                         info->fix.type = FB_TYPE_PACKED_PIXELS;
 257                         info->fix.line_length = info->var.xres_virtual;
 258                 }
 259         }
 260 }
 261
 262 static void vga16fb_clock_chip(struct vga16fb_par *par,
 263                                unsigned int *pixclock,
 264                                const struct fb_info *info,
 265                                int mul, int div)
 266 {
 267         static const struct {
 268                 u32 pixclock;
 269                 u8  misc;
 270                 u8  seq_clock_mode;
 271         } *ptr, *best, vgaclocks[] = {
 272                 { 79442 /* 12.587 */, 0x00, 0x08},
 273                 { 70616 /* 14.161 */, 0x04, 0x08},
 274                 { 39721 /* 25.175 */, 0x00, 0x00},
 275                 { 35308 /* 28.322 */, 0x04, 0x00},
 276                 {     0 /* bad */,    0x00, 0x00}};
 277         int err;
 278
 279         *pixclock = (*pixclock * mul) / div;
 280         best = vgaclocks;
 281         err = *pixclock - best->pixclock;
 282         if (err < 0) err = -err;
 283         for (ptr = vgaclocks + 1; ptr->pixclock; ptr++) {
 284                 int tmp;
 285
 286                 tmp = *pixclock - ptr->pixclock;
 287                 if (tmp < 0) tmp = -tmp;
 288                 if (tmp < err) {
 289                         err = tmp;
 290                         best = ptr;
 291                 }
 292         }
 293         par->misc |= best->misc;
 294         par->clkdiv = best->seq_clock_mode;
 295         *pixclock = (best->pixclock * div) / mul;
 296 }
 297
 298 #define FAIL(X) return -EINVAL
 299
 300 static int vga16fb_open(struct fb_info *info, int user)
 301 {
 302         struct vga16fb_par *par = info->par;
 303
 304         if (!par->ref_count) {
 305                 memset(&par->state, 0, sizeof(struct vgastate));
 306                 par->state.flags = VGA_SAVE_FONTS | VGA_SAVE_MODE |
 307                         VGA_SAVE_CMAP;
 308                 save_vga(&par->state);
 309         }
 310         par->ref_count++;
 311
 312         return 0;
 313 }
 314
 315 static int vga16fb_release(struct fb_info *info, int user)
 316 {
 317         struct vga16fb_par *par = info->par;
 318
 319         if (!par->ref_count)
 320                 return -EINVAL;
 321
 322         if (par->ref_count == 1)
 323                 restore_vga(&par->state);
 324         par->ref_count--;
 325
 326         return 0;
 327 }
 328
 329 static int vga16fb_check_var(struct fb_var_screeninfo *var,
 330                              struct fb_info *info)
 331 {
 332         struct vga16fb_par *par = info->par;
 333         u32 xres, right, hslen, left, xtotal;
 334         u32 yres, lower, vslen, upper, ytotal;
 335         u32 vxres, xoffset, vyres, yoffset;
 336         u32 pos;
 337         u8 r7, rMode;
 338         int shift;
 339         int mode;
 340         u32 maxmem;
 341
 342         par->pel_msk = 0xFF;
 343
 344         if (var->bits_per_pixel == 4) {
 345                 if (var->nonstd) {
 346                         if (!par->isVGA)
 347                                 return -EINVAL;
 348                         shift = 3;
 349                         mode = MODE_SKIP4 | MODE_CFB;
 350                         maxmem = 16384;
 351                         par->pel_msk = 0x0F;
 352                 } else {
 353                         shift = 3;
 354                         mode = 0;
 355                         maxmem = 65536;
 356                 }
 357         } else if (var->bits_per_pixel == 8) {
 358                 if (!par->isVGA)
 359                         return -EINVAL; /* no support on EGA */
 360                 shift = 2;
 361                 if (var->nonstd) {
 362                         mode = MODE_8BPP | MODE_CFB;
 363                         maxmem = 65536;
 364                 } else {
 365                         mode = MODE_SKIP4 | MODE_8BPP | MODE_CFB;
 366                         maxmem = 16384;
 367                 }
 368         } else
 369                 return -EINVAL;
 370
 371         xres = (var->xres + 7) & ~7;
 372         vxres = (var->xres_virtual + 0xF) & ~0xF;
 373         xoffset = (var->xoffset + 7) & ~7;
 374         left = (var->left_margin + 7) & ~7;
 375         right = (var->right_margin + 7) & ~7;
 376         hslen = (var->hsync_len + 7) & ~7;
 377
 378         if (vxres < xres)
 379                 vxres = xres;
 380         if (xres + xoffset > vxres)
 381                 xoffset = vxres - xres;
 382
 383         var->xres = xres;
 384         var->right_margin = right;
 385         var->hsync_len = hslen;
 386         var->left_margin = left;
 387         var->xres_virtual = vxres;
 388         var->xoffset = xoffset;
 389
 390         xres >>= shift;
 391         right >>= shift;
 392         hslen >>= shift;
 393         left >>= shift;
 394         vxres >>= shift;
 395         xtotal = xres + right + hslen + left;
 396         if (xtotal >= 256)
 397                 FAIL("xtotal too big");
 398         if (hslen > 32)
 399                 FAIL("hslen too big");
 400         if (right + hslen + left > 64)
 401                 FAIL("hblank too big");
 402         par->crtc[VGA_CRTC_H_TOTAL] = xtotal - 5;
 403         par->crtc[VGA_CRTC_H_BLANK_START] = xres - 1;
 404         par->crtc[VGA_CRTC_H_DISP] = xres - 1;
 405         pos = xres + right;
 406         par->crtc[VGA_CRTC_H_SYNC_START] = pos;
 407         pos += hslen;
 408         par->crtc[VGA_CRTC_H_SYNC_END] = pos & 0x1F;
 409         pos += left - 2; /* blank_end + 2 <= total + 5 */
 410         par->crtc[VGA_CRTC_H_BLANK_END] = (pos & 0x1F) | 0x80;
 411         if (pos & 0x20)
 412                 par->crtc[VGA_CRTC_H_SYNC_END] |= 0x80;
 413
 414         yres = var->yres;
 415         lower = var->lower_margin;
 416         vslen = var->vsync_len;
 417         upper = var->upper_margin;
 418         vyres = var->yres_virtual;
 419         yoffset = var->yoffset;
 420
 421         if (yres > vyres)
 422                 vyres = yres;
 423         if (vxres * vyres > maxmem) {
 424                 vyres = maxmem / vxres;
 425                 if (vyres < yres)
 426                         return -ENOMEM;
 427         }
 428         if (yoffset + yres > vyres)
 429                 yoffset = vyres - yres;
 430         var->yres = yres;
 431         var->lower_margin = lower;
 432         var->vsync_len = vslen;
 433         var->upper_margin = upper;
 434         var->yres_virtual = vyres;
 435         var->yoffset = yoffset;
 436
 437         if (var->vmode & FB_VMODE_DOUBLE) {
 438                 yres <<= 1;
 439                 lower <<= 1;
 440                 vslen <<= 1;
 441                 upper <<= 1;
 442         }
 443         ytotal = yres + lower + vslen + upper;
 444         if (ytotal > 1024) {
 445                 ytotal >>= 1;
 446                 yres >>= 1;
 447                 lower >>= 1;
 448                 vslen >>= 1;
 449                 upper >>= 1;
 450                 rMode = 0x04;
 451         } else
 452                 rMode = 0x00;
 453         if (ytotal > 1024)
 454                 FAIL("ytotal too big");
 455         if (vslen > 16)
 456                 FAIL("vslen too big");
 457         par->crtc[VGA_CRTC_V_TOTAL] = ytotal - 2;
 458         r7 = 0x10;      /* disable linecompare */
 459         if (ytotal & 0x100) r7 |= 0x01;
 460         if (ytotal & 0x200) r7 |= 0x20;
 461         par->crtc[VGA_CRTC_PRESET_ROW] = 0;
 462         par->crtc[VGA_CRTC_MAX_SCAN] = 0x40;    /* 1 scanline, no linecmp */
 463         if (var->vmode & FB_VMODE_DOUBLE)
 464                 par->crtc[VGA_CRTC_MAX_SCAN] |= 0x80;
 465         par->crtc[VGA_CRTC_CURSOR_START] = 0x20;
 466         par->crtc[VGA_CRTC_CURSOR_END]   = 0x00;
 467         if ((mode & (MODE_CFB | MODE_8BPP)) == MODE_CFB)
 468                 xoffset--;
 469         pos = yoffset * vxres + (xoffset >> shift);
 470         par->crtc[VGA_CRTC_START_HI]     = pos >> 8;
 471         par->crtc[VGA_CRTC_START_LO]     = pos & 0xFF;
 472         par->crtc[VGA_CRTC_CURSOR_HI]    = 0x00;
 473         par->crtc[VGA_CRTC_CURSOR_LO]    = 0x00;
 474         pos = yres - 1;
 475         par->crtc[VGA_CRTC_V_DISP_END] = pos & 0xFF;
 476         par->crtc[VGA_CRTC_V_BLANK_START] = pos & 0xFF;
 477         if (pos & 0x100)
 478                 r7 |= 0x0A;     /* 0x02 -> DISP_END, 0x08 -> BLANK_START */
 479         if (pos & 0x200) {
 480                 r7 |= 0x40;     /* 0x40 -> DISP_END */
 481                 par->crtc[VGA_CRTC_MAX_SCAN] |= 0x20; /* BLANK_START */
 482         }
 483         pos += lower;
 484         par->crtc[VGA_CRTC_V_SYNC_START] = pos & 0xFF;
 485         if (pos & 0x100)
 486                 r7 |= 0x04;
 487         if (pos & 0x200)
 488                 r7 |= 0x80;
 489         pos += vslen;
 490         par->crtc[VGA_CRTC_V_SYNC_END] = (pos & 0x0F) & ~0x10; /* disabled IRQ */
 491         pos += upper - 1; /* blank_end + 1 <= ytotal + 2 */
 492         par->crtc[VGA_CRTC_V_BLANK_END] = pos & 0xFF; /* 0x7F for original VGA,
 493                      but some SVGA chips requires all 8 bits to set */
 494         if (vxres >= 512)
 495                 FAIL("vxres too long");
 496         par->crtc[VGA_CRTC_OFFSET] = vxres >> 1;
 497         if (mode & MODE_SKIP4)
 498                 par->crtc[VGA_CRTC_UNDERLINE] = 0x5F;   /* 256, cfb8 */
 499         else
 500                 par->crtc[VGA_CRTC_UNDERLINE] = 0x1F;   /* 16, vgap */
 501         par->crtc[VGA_CRTC_MODE] = rMode | ((mode & MODE_TEXT) ? 0xA3 : 0xE3);
 502         par->crtc[VGA_CRTC_LINE_COMPARE] = 0xFF;
 503         par->crtc[VGA_CRTC_OVERFLOW] = r7;
 504
 505         par->vss = 0x00;        /* 3DA */
 506
 507         par->misc = 0xE3;       /* enable CPU, ports 0x3Dx, positive sync */
 508         if (var->sync & FB_SYNC_HOR_HIGH_ACT)
 509                 par->misc &= ~0x40;
 510         if (var->sync & FB_SYNC_VERT_HIGH_ACT)
 511                 par->misc &= ~0x80;
 512
 513         par->mode = mode;
 514
 515         if (mode & MODE_8BPP)
 516                 /* pixel clock == vga clock / 2 */
 517                 vga16fb_clock_chip(par, &var->pixclock, info, 1, 2);
 518         else
 519                 /* pixel clock == vga clock */
 520                 vga16fb_clock_chip(par, &var->pixclock, info, 1, 1);
 521
 522         var->red.offset = var->green.offset = var->blue.offset =
 523         var->transp.offset = 0;
 524         var->red.length = var->green.length = var->blue.length =
 525                 (par->isVGA) ? 6 : 2;
 526         var->transp.length = 0;
 527         var->activate = FB_ACTIVATE_NOW;
 528         var->height = -1;
 529         var->width = -1;
 530         var->accel_flags = 0;
 531         return 0;
 532 }
 533 #undef FAIL
 534
 535 static int vga16fb_set_par(struct fb_info *info)
 536 {
 537         struct vga16fb_par *par = info->par;
 538         u8 gdc[VGA_GFX_C];
 539         u8 seq[VGA_SEQ_C];
 540         u8 atc[VGA_ATT_C];
 541         int fh, i;
 542
 543         seq[VGA_SEQ_CLOCK_MODE] = 0x01 | par->clkdiv;
 544         if (par->mode & MODE_TEXT)
 545                 seq[VGA_SEQ_PLANE_WRITE] = 0x03;
 546         else
 547                 seq[VGA_SEQ_PLANE_WRITE] = 0x0F;
 548         seq[VGA_SEQ_CHARACTER_MAP] = 0x00;
 549         if (par->mode & MODE_TEXT)
 550                 seq[VGA_SEQ_MEMORY_MODE] = 0x03;
 551         else if (par->mode & MODE_SKIP4)
 552                 seq[VGA_SEQ_MEMORY_MODE] = 0x0E;
 553         else
 554                 seq[VGA_SEQ_MEMORY_MODE] = 0x06;
 555
 556         gdc[VGA_GFX_SR_VALUE] = 0x00;
 557         gdc[VGA_GFX_SR_ENABLE] = 0x00;
 558         gdc[VGA_GFX_COMPARE_VALUE] = 0x00;
 559         gdc[VGA_GFX_DATA_ROTATE] = 0x00;
 560         gdc[VGA_GFX_PLANE_READ] = 0;
 561         if (par->mode & MODE_TEXT) {
 562                 gdc[VGA_GFX_MODE] = 0x10;
 563                 gdc[VGA_GFX_MISC] = 0x06;
 564         } else {
 565                 if (par->mode & MODE_CFB)
 566                         gdc[VGA_GFX_MODE] = 0x40;
 567                 else
 568                         gdc[VGA_GFX_MODE] = 0x00;
 569                 gdc[VGA_GFX_MISC] = 0x05;
 570         }
 571         gdc[VGA_GFX_COMPARE_MASK] = 0x0F;
 572         gdc[VGA_GFX_BIT_MASK] = 0xFF;
 573
 574         for (i = 0x00; i < 0x10; i++)
 575                 atc[i] = i;
 576         if (par->mode & MODE_TEXT)
 577                 atc[VGA_ATC_MODE] = 0x04;
 578         else if (par->mode & MODE_8BPP)
 579                 atc[VGA_ATC_MODE] = 0x41;
 580         else
 581                 atc[VGA_ATC_MODE] = 0x81;
 582         atc[VGA_ATC_OVERSCAN] = 0x00;   /* 0 for EGA, 0xFF for VGA */
 583         atc[VGA_ATC_PLANE_ENABLE] = 0x0F;
 584         if (par->mode & MODE_8BPP)
 585                 atc[VGA_ATC_PEL] = (info->var.xoffset & 3) << 1;
 586         else
 587                 atc[VGA_ATC_PEL] = info->var.xoffset & 7;
 588         atc[VGA_ATC_COLOR_PAGE] = 0x00;
 589
 590         if (par->mode & MODE_TEXT) {
 591                 fh = 16; // FIXME !!! Fudge font height.
 592                 par->crtc[VGA_CRTC_MAX_SCAN] = (par->crtc[VGA_CRTC_MAX_SCAN]
 593                                                & ~0x1F) | (fh - 1);
 594         }
 595
 596         vga_io_w(VGA_MIS_W, vga_io_r(VGA_MIS_R) | 0x01);
 597
 598         /* Enable graphics register modification */
 599         if (!par->isVGA) {
 600                 vga_io_w(EGA_GFX_E0, 0x00);
 601                 vga_io_w(EGA_GFX_E1, 0x01);
 602         }
 603
 604         /* update misc output register */
 605         vga_io_w(VGA_MIS_W, par->misc);
 606
 607         /* synchronous reset on */
 608         vga_io_wseq(0x00, 0x01);
 609
 610         if (par->isVGA)
 611                 vga_io_w(VGA_PEL_MSK, par->pel_msk);
 612
 613         /* write sequencer registers */
 614         vga_io_wseq(VGA_SEQ_CLOCK_MODE, seq[VGA_SEQ_CLOCK_MODE] | 0x20);
 615         for (i = 2; i < VGA_SEQ_C; i++) {
 616                 vga_io_wseq(i, seq[i]);
 617         }
 618
 619         /* synchronous reset off */
 620         vga_io_wseq(0x00, 0x03);
 621
 622         /* deprotect CRT registers 0-7 */
 623         vga_io_wcrt(VGA_CRTC_V_SYNC_END, par->crtc[VGA_CRTC_V_SYNC_END]);
 624
 625         /* write CRT registers */
 626         for (i = 0; i < VGA_CRTC_REGS; i++) {
 627                 vga_io_wcrt(i, par->crtc[i]);
 628         }
 629
 630         /* write graphics controller registers */
 631         for (i = 0; i < VGA_GFX_C; i++) {
 632                 vga_io_wgfx(i, gdc[i]);
 633         }
 634
 635         /* write attribute controller registers */
 636         for (i = 0; i < VGA_ATT_C; i++) {
 637                 vga_io_r(VGA_IS1_RC);           /* reset flip-flop */
 638                 vga_io_wattr(i, atc[i]);
 639         }
 640
 641         /* Wait for screen to stabilize. */
 642         mdelay(50);
 643
 644         vga_io_wseq(VGA_SEQ_CLOCK_MODE, seq[VGA_SEQ_CLOCK_MODE]);
 645
 646         vga_io_r(VGA_IS1_RC);
 647         vga_io_w(VGA_ATT_IW, 0x20);
 648
 649         vga16fb_update_fix(info);
 650         return 0;
 651 }
 652
 653 static void ega16_setpalette(int regno, unsigned red, unsigned green, unsigned blue)
 654 {
 655         static const unsigned char map[] = { 000, 001, 010, 011 };
 656         int val;
 657
 658         if (regno >= 16)
 659                 return;
 660         val = map[red>>14] | ((map[green>>14]) << 1) | ((map[blue>>14]) << 2);
 661         vga_io_r(VGA_IS1_RC);   /* ! 0x3BA */
 662         vga_io_wattr(regno, val);
 663         vga_io_r(VGA_IS1_RC);   /* some clones need it */
 664         vga_io_w(VGA_ATT_IW, 0x20); /* unblank screen */
 665 }
 666
 667 static void vga16_setpalette(int regno, unsigned red, unsigned green, unsigned blue)
 668 {
 669         outb(regno,       VGA_PEL_IW);
 670         outb(red   >> 10, VGA_PEL_D);
 671         outb(green >> 10, VGA_PEL_D);
 672         outb(blue  >> 10, VGA_PEL_D);
 673 }
 674
 675 static int vga16fb_setcolreg(unsigned regno, unsigned red, unsigned green,
 676                              unsigned blue, unsigned transp,
 677                              struct fb_info *info)
 678 {
 679         struct vga16fb_par *par = info->par;
 680         int gray;
 681
 682         /*
 683          *  Set a single color register. The values supplied are
 684          *  already rounded down to the hardware's capabilities
 685          *  (according to the entries in the `var' structure). Return
 686          *  != 0 for invalid regno.
 687          */
 688
 689         if (regno >= 256)
 690                 return 1;
 691
 692         gray = info->var.grayscale;
 693
 694         if (gray) {
 695                 /* gray = 0.30*R + 0.59*G + 0.11*B */
 696                 red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
 697         }
 698         if (par->isVGA)
 699                 vga16_setpalette(regno,red,green,blue);
 700         else
 701                 ega16_setpalette(regno,red,green,blue);
 702         return 0;
 703 }
 704
 705 static int vga16fb_pan_display(struct fb_var_screeninfo *var,
 706                                struct fb_info *info)
 707 {
 708         vga16fb_pan_var(info, var);
 709         return 0;
 710 }
 711
 712 /* The following VESA blanking code is taken from vgacon.c.  The VGA
 713    blanking code was originally by Huang shi chao, and modified by
 714    Christoph Rimek ([email protected]) and todd j. derr
 715    ([email protected]) for Linux. */
 716
 717 static void vga_vesa_blank(struct vga16fb_par *par, int mode)
 718 {
 719         unsigned char SeqCtrlIndex = vga_io_r(VGA_SEQ_I);
 720         unsigned char CrtCtrlIndex = vga_io_r(VGA_CRT_IC);
 721
 722         /* save original values of VGA controller registers */
 723         if(!par->vesa_blanked) {
 724                 par->vga_state.CrtMiscIO = vga_io_r(VGA_MIS_R);
 725                 //sti();
 726
 727                 par->vga_state.HorizontalTotal = vga_io_rcrt(0x00);     /* HorizontalTotal */
 728                 par->vga_state.HorizDisplayEnd = vga_io_rcrt(0x01);     /* HorizDisplayEnd */
 729                 par->vga_state.StartHorizRetrace = vga_io_rcrt(0x04);   /* StartHorizRetrace */
 730                 par->vga_state.EndHorizRetrace = vga_io_rcrt(0x05);     /* EndHorizRetrace */
 731                 par->vga_state.Overflow = vga_io_rcrt(0x07);            /* Overflow */
 732                 par->vga_state.StartVertRetrace = vga_io_rcrt(0x10);    /* StartVertRetrace */
 733                 par->vga_state.EndVertRetrace = vga_io_rcrt(0x11);      /* EndVertRetrace */
 734                 par->vga_state.ModeControl = vga_io_rcrt(0x17); /* ModeControl */
 735                 par->vga_state.ClockingMode = vga_io_rseq(0x01);        /* ClockingMode */
 736         }
 737
 738         /* assure that video is enabled */
 739         /* "0x20" is VIDEO_ENABLE_bit in register 01 of sequencer */
 740         vga_io_wseq(0x01, par->vga_state.ClockingMode | 0x20);
 741
 742         /* test for vertical retrace in process.... */
 743         if ((par->vga_state.CrtMiscIO & 0x80) == 0x80)
 744                 vga_io_w(VGA_MIS_W, par->vga_state.CrtMiscIO & 0xef);
 745
 746         /*
 747          * Set <End of vertical retrace> to minimum (0) and
 748          * <Start of vertical Retrace> to maximum (incl. overflow)
 749          * Result: turn off vertical sync (VSync) pulse.
 750          */
 751         if (mode & FB_BLANK_VSYNC_SUSPEND) {
 752                 vga_io_wcrt(VGA_CRTC_V_SYNC_START, 0xff);
 753                 vga_io_wcrt(VGA_CRTC_V_SYNC_END, 0x40);
 754                 /* bits 9,10 of vert. retrace */
 755                 vga_io_wcrt(VGA_CRTC_OVERFLOW, par->vga_state.Overflow | 0x84);
 756         }
 757
 758         if (mode & FB_BLANK_HSYNC_SUSPEND) {
 759                 /*
 760                  * Set <End of horizontal retrace> to minimum (0) and
 761                  *  <Start of horizontal Retrace> to maximum
 762                  * Result: turn off horizontal sync (HSync) pulse.
 763                  */
 764                 vga_io_wcrt(VGA_CRTC_H_SYNC_START, 0xff);
 765                 vga_io_wcrt(VGA_CRTC_H_SYNC_END, 0x00);
 766         }
 767
 768         /* restore both index registers */
 769         outb_p(SeqCtrlIndex, VGA_SEQ_I);
 770         outb_p(CrtCtrlIndex, VGA_CRT_IC);
 771 }
 772
 773 static void vga_vesa_unblank(struct vga16fb_par *par)
 774 {
 775         unsigned char SeqCtrlIndex = vga_io_r(VGA_SEQ_I);
 776         unsigned char CrtCtrlIndex = vga_io_r(VGA_CRT_IC);
 777
 778         /* restore original values of VGA controller registers */
 779         vga_io_w(VGA_MIS_W, par->vga_state.CrtMiscIO);
 780
 781         /* HorizontalTotal */
 782         vga_io_wcrt(0x00, par->vga_state.HorizontalTotal);
 783         /* HorizDisplayEnd */
 784         vga_io_wcrt(0x01, par->vga_state.HorizDisplayEnd);
 785         /* StartHorizRetrace */
 786         vga_io_wcrt(0x04, par->vga_state.StartHorizRetrace);
 787         /* EndHorizRetrace */
 788         vga_io_wcrt(0x05, par->vga_state.EndHorizRetrace);
 789         /* Overflow */
 790         vga_io_wcrt(0x07, par->vga_state.Overflow);
 791         /* StartVertRetrace */
 792         vga_io_wcrt(0x10, par->vga_state.StartVertRetrace);
 793         /* EndVertRetrace */
 794         vga_io_wcrt(0x11, par->vga_state.EndVertRetrace);
 795         /* ModeControl */
 796         vga_io_wcrt(0x17, par->vga_state.ModeControl);
 797         /* ClockingMode */
 798         vga_io_wseq(0x01, par->vga_state.ClockingMode);
 799
 800         /* restore index/control registers */
 801         vga_io_w(VGA_SEQ_I, SeqCtrlIndex);
 802         vga_io_w(VGA_CRT_IC, CrtCtrlIndex);
 803 }
 804
 805 static void vga_pal_blank(void)
 806 {
 807         int i;
 808
 809         for (i=0; i<16; i++) {
 810                 outb_p(i, VGA_PEL_IW);
 811                 outb_p(0, VGA_PEL_D);
 812                 outb_p(0, VGA_PEL_D);
 813                 outb_p(0, VGA_PEL_D);
 814         }
 815 }
 816
 817 /* 0 unblank, 1 blank, 2 no vsync, 3 no hsync, 4 off */
 818 static int vga16fb_blank(int blank, struct fb_info *info)
 819 {
 820         struct vga16fb_par *par = info->par;
 821
 822         switch (blank) {
 823         case FB_BLANK_UNBLANK:                          /* Unblank */
 824                 if (par->vesa_blanked) {
 825                         vga_vesa_unblank(par);
 826                         par->vesa_blanked = 0;
 827                 }
 828                 if (par->palette_blanked) {
 829                         par->palette_blanked = 0;
 830                 }
 831                 break;
 832         case FB_BLANK_NORMAL:                           /* blank */
 833                 vga_pal_blank();
 834                 par->palette_blanked = 1;
 835                 break;
 836         default:                        /* VESA blanking */
 837                 vga_vesa_blank(par, blank);
 838                 par->vesa_blanked = 1;
 839                 break;
 840         }
 841         return 0;
 842 }
 843
 844 static void vga_8planes_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 845 {
 846         u32 dx = rect->dx, width = rect->width;
 847         char oldindex = getindex();
 848         char oldmode = setmode(0x40);
 849         char oldmask = selectmask();
 850         int line_ofs, height;
 851         char oldop, oldsr;
 852         char __iomem *where;
 853
 854         dx /= 4;
 855         where = info->screen_base + dx + rect->dy * info->fix.line_length;
 856
 857         if (rect->rop == ROP_COPY) {
 858                 oldop = setop(0);
 859                 oldsr = setsr(0);
 860
 861                 width /= 4;
 862                 line_ofs = info->fix.line_length - width;
 863                 setmask(0xff);
 864
 865                 height = rect->height;
 866
 867                 while (height--) {
 868                         int x;
 869
 870                         /* we can do memset... */
 871                         for (x = width; x > 0; --x) {
 872                                 writeb(rect->color, where);
 873                                 where++;
 874                         }
 875                         where += line_ofs;
 876                 }
 877         } else {
 878                 char oldcolor = setcolor(0xf);
 879                 int y;
 880
 881                 oldop = setop(0x18);
 882                 oldsr = setsr(0xf);
 883                 setmask(0x0F);
 884                 for (y = 0; y < rect->height; y++) {
 885                         rmw(where);
 886                         rmw(where+1);
 887                         where += info->fix.line_length;
 888                 }
 889                 setcolor(oldcolor);
 890         }
 891         setmask(oldmask);
 892         setsr(oldsr);
 893         setop(oldop);
 894         setmode(oldmode);
 895         setindex(oldindex);
 896 }
 897
 898 static void vga16fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 899 {
 900         int x, x2, y2, vxres, vyres, width, height, line_ofs;
 901         char __iomem *dst;
 902
 903         vxres = info->var.xres_virtual;
 904         vyres = info->var.yres_virtual;
 905
 906         if (!rect->width || !rect->height || rect->dx > vxres || rect->dy > vyres)
 907                 return;
 908
 909         /* We could use hardware clipping but on many cards you get around
 910          * hardware clipping by writing to framebuffer directly. */
 911
 912         x2 = rect->dx + rect->width;
 913         y2 = rect->dy + rect->height;
 914         x2 = x2 < vxres ? x2 : vxres;
 915         y2 = y2 < vyres ? y2 : vyres;
 916         width = x2 - rect->dx;
 917
 918         switch (info->fix.type) {
 919         case FB_TYPE_VGA_PLANES:
 920                 if (info->fix.type_aux == FB_AUX_VGA_PLANES_VGA4) {
 921
 922                         height = y2 - rect->dy;
 923                         width = rect->width/8;
 924
 925                         line_ofs = info->fix.line_length - width;
 926                         dst = info->screen_base + (rect->dx/8) + rect->dy * info->fix.line_length;
 927
 928                         switch (rect->rop) {
 929                         case ROP_COPY:
 930                                 setmode(0);
 931                                 setop(0);
 932                                 setsr(0xf);
 933                                 setcolor(rect->color);
 934                                 selectmask();
 935
 936                                 setmask(0xff);
 937
 938                                 while (height--) {
 939                                         for (x = 0; x < width; x++) {
 940                                                 writeb(0, dst);
 941                                                 dst++;
 942                                         }
 943                                         dst += line_ofs;
 944                                 }
 945                                 break;
 946                         case ROP_XOR:
 947                                 setmode(0);
 948                                 setop(0x18);
 949                                 setsr(0xf);
 950                                 setcolor(0xf);
 951                                 selectmask();
 952
 953                                 setmask(0xff);
 954                                 while (height--) {
 955                                         for (x = 0; x < width; x++) {
 956                                                 rmw(dst);
 957                                                 dst++;
 958                                         }
 959                                         dst += line_ofs;
 960                                 }
 961                                 break;
 962                         }
 963                 } else
 964                         vga_8planes_fillrect(info, rect);
 965                 break;
 966         case FB_TYPE_PACKED_PIXELS:
 967         default:
 968                 cfb_fillrect(info, rect);
 969                 break;
 970         }
 971 }
 972
 973 static void vga_8planes_copyarea(struct fb_info *info, const struct fb_copyarea *area)
 974 {
 975         char oldindex = getindex();
 976         char oldmode = setmode(0x41);
 977         char oldop = setop(0);
 978         char oldsr = setsr(0xf);
 979         int height, line_ofs, x;
 980         u32 sx, dx, width;
 981         char __iomem *dest;
 982         char __iomem *src;
 983
 984         height = area->height;
 985
 986         sx = area->sx / 4;
 987         dx = area->dx / 4;
 988         width = area->width / 4;
 989
 990         if (area->dy < area->sy || (area->dy == area->sy && dx < sx)) {
 991                 line_ofs = info->fix.line_length - width;
 992                 dest = info->screen_base + dx + area->dy * info->fix.line_length;
 993                 src = info->screen_base + sx + area->sy * info->fix.line_length;
 994                 while (height--) {
 995                         for (x = 0; x < width; x++) {
 996                                 readb(src);
 997                                 writeb(0, dest);
 998                                 src++;
 999                                 dest++;
1000                         }
1001                         src += line_ofs;
1002                         dest += line_ofs;
1003                 }
1004         } else {
1005                 line_ofs = info->fix.line_length - width;
1006                 dest = info->screen_base + dx + width +
1007                         (area->dy + height - 1) * info->fix.line_length;
1008                 src = info->screen_base + sx + width +
1009                         (area->sy + height - 1) * info->fix.line_length;
1010                 while (height--) {
1011                         for (x = 0; x < width; x++) {
1012                                 --src;
1013                                 --dest;
1014                                 readb(src);
1015                                 writeb(0, dest);
1016                         }
1017                         src -= line_ofs;
1018                         dest -= line_ofs;
1019                 }
1020         }
1021
1022         setsr(oldsr);
1023         setop(oldop);
1024         setmode(oldmode);
1025         setindex(oldindex);
1026 }
1027
1028 static void vga16fb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
1029 {
1030         u32 dx = area->dx, dy = area->dy, sx = area->sx, sy = area->sy;
1031         int x, x2, y2, old_dx, old_dy, vxres, vyres;
1032         int height, width, line_ofs;
1033         char __iomem *dst = NULL;
1034         char __iomem *src = NULL;
1035
1036         vxres = info->var.xres_virtual;
1037         vyres = info->var.yres_virtual;
1038
1039         if (area->dx > vxres || area->sx > vxres || area->dy > vyres ||
1040             area->sy > vyres)
1041                 return;
1042
1043         /* clip the destination */
1044         old_dx = area->dx;
1045         old_dy = area->dy;
1046
1047         /*
1048          * We could use hardware clipping but on many cards you get around
1049          * hardware clipping by writing to framebuffer directly.
1050          */
1051         x2 = area->dx + area->width;
1052         y2 = area->dy + area->height;
1053         dx = area->dx > 0 ? area->dx : 0;
1054         dy = area->dy > 0 ? area->dy : 0;
1055         x2 = x2 < vxres ? x2 : vxres;
1056         y2 = y2 < vyres ? y2 : vyres;
1057         width = x2 - dx;
1058         height = y2 - dy;
1059
1060         if (sx + dx < old_dx || sy + dy < old_dy)
1061                 return;
1062
1063         /* update sx1,sy1 */
1064         sx += (dx - old_dx);
1065         sy += (dy - old_dy);
1066
1067         /* the source must be completely inside the virtual screen */
1068         if (sx + width > vxres || sy + height > vyres)
1069                 return;
1070
1071         switch (info->fix.type) {
1072         case FB_TYPE_VGA_PLANES:
1073                 if (info->fix.type_aux == FB_AUX_VGA_PLANES_VGA4) {
1074                         width = width/8;
1075                         line_ofs = info->fix.line_length - width;
1076
1077                         setmode(1);
1078                         setop(0);
1079                         setsr(0xf);
1080
1081                         if (dy < sy || (dy == sy && dx < sx)) {
1082                                 dst = info->screen_base + (dx/8) + dy * info->fix.line_length;
1083                                 src = info->screen_base + (sx/8) + sy * info->fix.line_length;
1084                                 while (height--) {
1085                                         for (x = 0; x < width; x++) {
1086                                                 readb(src);
1087                                                 writeb(0, dst);
1088                                                 dst++;
1089                                                 src++;
1090                                         }
1091                                         src += line_ofs;
1092                                         dst += line_ofs;
1093                                 }
1094                         } else {
1095                                 dst = info->screen_base + (dx/8) + width +
1096                                         (dy + height - 1) * info->fix.line_length;
1097                                 src = info->screen_base + (sx/8) + width +
1098                                         (sy + height  - 1) * info->fix.line_length;
1099                                 while (height--) {
1100                                         for (x = 0; x < width; x++) {
1101                                                 dst--;
1102                                                 src--;
1103                                                 readb(src);
1104                                                 writeb(0, dst);
1105                                         }
1106                                         src -= line_ofs;
1107                                         dst -= line_ofs;
1108                                 }
1109                         }
1110                 } else
1111                         vga_8planes_copyarea(info, area);
1112                 break;
1113         case FB_TYPE_PACKED_PIXELS:
1114         default:
1115                 cfb_copyarea(info, area);
1116                 break;
1117         }
1118 }
1119
1120 #define TRANS_MASK_LOW  {0x0,0x8,0x4,0xC,0x2,0xA,0x6,0xE,0x1,0x9,0x5,0xD,0x3,0xB,0x7,0xF}
1121 #define TRANS_MASK_HIGH {0x000, 0x800, 0x400, 0xC00, 0x200, 0xA00, 0x600, 0xE00, \
1122                          0x100, 0x900, 0x500, 0xD00, 0x300, 0xB00, 0x700, 0xF00}
1123
1124 #if defined(__LITTLE_ENDIAN)
1125 static const u16 transl_l[] = TRANS_MASK_LOW;
1126 static const u16 transl_h[] = TRANS_MASK_HIGH;
1127 #elif defined(__BIG_ENDIAN)
1128 static const u16 transl_l[] = TRANS_MASK_HIGH;
1129 static const u16 transl_h[] = TRANS_MASK_LOW;
1130 #else
1131 #error "Only __BIG_ENDIAN and __LITTLE_ENDIAN are supported in vga-planes"
1132 #endif
1133
1134 static void vga_8planes_imageblit(struct fb_info *info, const struct fb_image *image)
1135 {
1136         char oldindex = getindex();
1137         char oldmode = setmode(0x40);
1138         char oldop = setop(0);
1139         char oldsr = setsr(0);
1140         char oldmask = selectmask();
1141         const unsigned char *cdat = image->data;
1142         u32 dx = image->dx;
1143         char __iomem *where;
1144         int y;
1145
1146         dx /= 4;
1147         where = info->screen_base + dx + image->dy * info->fix.line_length;
1148
1149         setmask(0xff);
1150         writeb(image->bg_color, where);
1151         readb(where);
1152         selectmask();
1153         setmask(image->fg_color ^ image->bg_color);
1154         setmode(0x42);
1155         setop(0x18);
1156         for (y = 0; y < image->height; y++, where += info->fix.line_length)
1157                 writew(transl_h[cdat[y]&0xF] | transl_l[cdat[y] >> 4], where);
1158         setmask(oldmask);
1159         setsr(oldsr);
1160         setop(oldop);
1161         setmode(oldmode);
1162         setindex(oldindex);
1163 }
1164
1165 static void vga_imageblit_expand(struct fb_info *info, const struct fb_image *image)
1166 {
1167         char __iomem *where = info->screen_base + (image->dx/8) +
1168                 image->dy * info->fix.line_length;
1169         struct vga16fb_par *par = info->par;
1170         char *cdat = (char *) image->data;
1171         char __iomem *dst;
1172         int x, y;
1173
1174         switch (info->fix.type) {
1175         case FB_TYPE_VGA_PLANES:
1176                 if (info->fix.type_aux == FB_AUX_VGA_PLANES_VGA4) {
1177                         if (par->isVGA) {
1178                                 setmode(2);
1179                                 setop(0);
1180                                 setsr(0xf);
1181                                 setcolor(image->fg_color);
1182                                 selectmask();
1183
1184                                 setmask(0xff);
1185                                 writeb(image->bg_color, where);
1186                                 rmb();
1187                                 readb(where); /* fill latches */
1188                                 setmode(3);
1189                                 wmb();
1190                                 for (y = 0; y < image->height; y++) {
1191                                         dst = where;
1192                                         for (x = image->width/8; x--;)
1193                                                 writeb(*cdat++, dst++);
1194                                         where += info->fix.line_length;
1195                                 }
1196                                 wmb();
1197                         } else {
1198                                 setmode(0);
1199                                 setop(0);
1200                                 setsr(0xf);
1201                                 setcolor(image->bg_color);
1202                                 selectmask();
1203
1204                                 setmask(0xff);
1205                                 for (y = 0; y < image->height; y++) {
1206                                         dst = where;
1207                                         for (x=image->width/8; x--;){
1208                                                 rmw(dst);
1209                                                 setcolor(image->fg_color);
1210                                                 selectmask();
1211                                                 if (*cdat) {
1212                                                         setmask(*cdat++);
1213                                                         rmw(dst++);
1214                                                 }
1215                                         }
1216                                         where += info->fix.line_length;
1217                                 }
1218                         }
1219                 } else
1220                         vga_8planes_imageblit(info, image);
1221                 break;
1222         case FB_TYPE_PACKED_PIXELS:
1223         default:
1224                 cfb_imageblit(info, image);
1225                 break;
1226         }
1227 }
1228
1229 static void vga_imageblit_color(struct fb_info *info, const struct fb_image *image)
1230 {
1231         /*
1232          * Draw logo
1233          */
1234         struct vga16fb_par *par = info->par;
1235         char __iomem *where =
1236                 info->screen_base + image->dy * info->fix.line_length +
1237                 image->dx/8;
1238         const char *cdat = image->data;
1239         char __iomem *dst;
1240         int x, y;
1241
1242         switch (info->fix.type) {
1243         case FB_TYPE_VGA_PLANES:
1244                 if (info->fix.type_aux == FB_AUX_VGA_PLANES_VGA4 &&
1245                     par->isVGA) {
1246                         setsr(0xf);
1247                         setop(0);
1248                         setmode(0);
1249
1250                         for (y = 0; y < image->height; y++) {
1251                                 for (x = 0; x < image->width; x++) {
1252                                         dst = where + x/8;
1253
1254                                         setcolor(*cdat);
1255                                         selectmask();
1256                                         setmask(1 << (7 - (x % 8)));
1257                                         fb_readb(dst);
1258                                         fb_writeb(0, dst);
1259
1260                                         cdat++;
1261                                 }
1262                                 where += info->fix.line_length;
1263                         }
1264                 }
1265                 break;
1266         case FB_TYPE_PACKED_PIXELS:
1267                 cfb_imageblit(info, image);
1268                 break;
1269         default:
1270                 break;
1271         }
1272 }
1273
1274 static void vga16fb_imageblit(struct fb_info *info, const struct fb_image *image)
1275 {
1276         if (image->depth == 1)
1277                 vga_imageblit_expand(info, image);
1278         else
1279                 vga_imageblit_color(info, image);
1280 }
1281
1282 static void vga16fb_destroy(struct fb_info *info)
1283 {
1284         iounmap(info->screen_base);
1285         fb_dealloc_cmap(&info->cmap);
1286         /* XXX unshare VGA regions */
1287         framebuffer_release(info);
1288 }
1289
1290 static const struct fb_ops vga16fb_ops = {
1291         .owner          = THIS_MODULE,
1292         .fb_open        = vga16fb_open,
1293         .fb_release     = vga16fb_release,
1294         .fb_destroy     = vga16fb_destroy,
1295         .fb_check_var   = vga16fb_check_var,
1296         .fb_set_par     = vga16fb_set_par,
1297         .fb_setcolreg   = vga16fb_setcolreg,
1298         .fb_pan_display = vga16fb_pan_display,
1299         .fb_blank       = vga16fb_blank,
1300         .fb_fillrect    = vga16fb_fillrect,
1301         .fb_copyarea    = vga16fb_copyarea,
1302         .fb_imageblit   = vga16fb_imageblit,
1303 };
1304
1305 static int vga16fb_probe(struct platform_device *dev)
1306 {
1307         struct screen_info *si;
1308         struct fb_info *info;
1309         struct vga16fb_par *par;
1310         int i;
1311         int ret = 0;
1312
1313         si = dev_get_platdata(&dev->dev);
1314         if (!si)
1315                 return -ENODEV;
1316
1317         ret = check_mode_supported(si);
1318         if (ret)
1319                 return ret;
1320
1321         printk(KERN_DEBUG "vga16fb: initializing\n");
1322         info = framebuffer_alloc(sizeof(struct vga16fb_par), &dev->dev);
1323
1324         if (!info) {
1325                 ret = -ENOMEM;
1326                 goto err_fb_alloc;
1327         }
1328
1329         /* XXX share VGA_FB_PHYS_BASE and I/O region with vgacon and others */
1330         info->screen_base = (void __iomem *)VGA_MAP_MEM(VGA_FB_PHYS_BASE, 0);
1331
1332         if (!info->screen_base) {
1333                 printk(KERN_ERR "vga16fb: unable to map device\n");
1334                 ret = -ENOMEM;
1335                 goto err_ioremap;
1336         }
1337
1338         printk(KERN_INFO "vga16fb: mapped to 0x%p\n", info->screen_base);
1339         par = info->par;
1340
1341 #if defined(CONFIG_X86)
1342         par->isVGA = si->orig_video_isVGA == VIDEO_TYPE_VGAC;
1343 #else
1344         /* non-x86 architectures treat orig_video_isVGA as a boolean flag */
1345         par->isVGA = si->orig_video_isVGA;
1346 #endif
1347         par->palette_blanked = 0;
1348         par->vesa_blanked = 0;
1349
1350         i = par->isVGA? 6 : 2;
1351
1352         vga16fb_defined.red.length   = i;
1353         vga16fb_defined.green.length = i;
1354         vga16fb_defined.blue.length  = i;
1355
1356         /* name should not depend on EGA/VGA */
1357         info->fbops = &vga16fb_ops;
1358         info->var = vga16fb_defined;
1359         info->fix = vga16fb_fix;
1360         /* supports rectangles with widths of multiples of 8 */
1361         info->pixmap.blit_x = 1 << 7 | 1 << 15 | 1 << 23 | 1 << 31;
1362         info->flags = FBINFO_FLAG_DEFAULT | FBINFO_HWACCEL_YPAN;
1363
1364         i = (info->var.bits_per_pixel == 8) ? 256 : 16;
1365         ret = fb_alloc_cmap(&info->cmap, i, 0);
1366         if (ret) {
1367                 printk(KERN_ERR "vga16fb: unable to allocate colormap\n");
1368                 ret = -ENOMEM;
1369                 goto err_alloc_cmap;
1370         }
1371
1372         if (vga16fb_check_var(&info->var, info)) {
1373                 printk(KERN_ERR "vga16fb: unable to validate variable\n");
1374                 ret = -EINVAL;
1375                 goto err_check_var;
1376         }
1377
1378         vga16fb_update_fix(info);
1379
1380         ret = devm_aperture_acquire_for_platform_device(dev, VGA_FB_PHYS_BASE, VGA_FB_PHYS_SIZE);
1381         if (ret)
1382                 goto err_check_var;
1383         if (register_framebuffer(info) < 0) {
1384                 printk(KERN_ERR "vga16fb: unable to register framebuffer\n");
1385                 ret = -EINVAL;
1386                 goto err_check_var;
1387         }
1388
1389         fb_info(info, "%s frame buffer device\n", info->fix.id);
1390         platform_set_drvdata(dev, info);
1391
1392         return 0;
1393
1394  err_check_var:
1395         fb_dealloc_cmap(&info->cmap);
1396  err_alloc_cmap:
1397         iounmap(info->screen_base);
1398  err_ioremap:
1399         framebuffer_release(info);
1400  err_fb_alloc:
1401         return ret;
1402 }
1403
1404 static void vga16fb_remove(struct platform_device *dev)
1405 {
1406         struct fb_info *info = platform_get_drvdata(dev);
1407
1408         if (info)
1409                 unregister_framebuffer(info);
1410 }
1411
1412 static const struct platform_device_id vga16fb_driver_id_table[] = {
1413         {"ega-framebuffer", 0},
1414         {"vga-framebuffer", 0},
1415         { }
1416 };
1417 MODULE_DEVICE_TABLE(platform, vga16fb_driver_id_table);
1418
1419 static struct platform_driver vga16fb_driver = {
1420         .probe = vga16fb_probe,
1421         .remove_new = vga16fb_remove,
1422         .driver = {
1423                 .name = "vga16fb",
1424         },
1425         .id_table = vga16fb_driver_id_table,
1426 };
1427
1428 module_platform_driver(vga16fb_driver);
1429
1430 MODULE_DESCRIPTION("Legacy VGA framebuffer device driver");
1431 MODULE_LICENSE("GPL");