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