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