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