drivers/video/fbdev/vga16fb.c

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