drivers/video/fbdev/vga16fb.c

   1 /*
   2  * linux/drivers/video/vga16.c -- VGA 16-color framebuffer driver
   3  *
   4  * Copyright 1999 Ben Pfaff <pfaffben@debian.org> and Petr Vandrovec <VANDROVE@vc.cvut.cz>
   5  * Based on VGA info at http://www.goodnet.com/~tinara/FreeVGA/home.htm
   6  * Based on VESA framebuffer (c) 1998 Gerd Knorr <kraxel@goldbach.in-berlin.de>
   7  *
   8  * This file is subject to the terms and conditions of the GNU General
   9  * Public License.  See the file COPYING in the main directory of this
  10  * archive for more details.
  11  */
  12
  13 #include <linux/module.h>
  14 #include <linux/kernel.h>
  15 #include <linux/errno.h>
  16 #include <linux/string.h>
  17 #include <linux/mm.h>
  18 #include <linux/delay.h>
  19 #include <linux/fb.h>
  20 #include <linux/ioport.h>
  21 #include <linux/init.h>
  22 #include <linux/platform_device.h>
  23 #include <linux/screen_info.h>
  24
  25 #include <asm/io.h>
  26 #include <video/vga.h>
  27
  28 #define VGA_FB_PHYS 0xA0000
  29 #define VGA_FB_PHYS_LEN 65536
  30
  31 #define MODE_SKIP4      1
  32 #define MODE_8BPP       2
  33 #define MODE_CFB        4
  34 #define MODE_TEXT       8
  35
  36 /* --------------------------------------------------------------------- */
  37
  38 /*
  39  * card parameters
  40  */
  41
  42 struct vga16fb_par {
  43         /* structure holding original VGA register settings when the
  44            screen is blanked */
  45         struct {
  46                 unsigned char   SeqCtrlIndex;     /* Sequencer Index reg.   */
  47                 unsigned char   CrtCtrlIndex;     /* CRT-Contr. Index reg.  */
  48                 unsigned char   CrtMiscIO;        /* Miscellaneous register */
  49                 unsigned char   HorizontalTotal;  /* CRT-Controller:00h */
  50                 unsigned char   HorizDisplayEnd;  /* CRT-Controller:01h */
  51                 unsigned char   StartHorizRetrace;/* CRT-Controller:04h */
  52                 unsigned char   EndHorizRetrace;  /* CRT-Controller:05h */
  53                 unsigned char   Overflow;         /* CRT-Controller:07h */
  54                 unsigned char   StartVertRetrace; /* CRT-Controller:10h */
  55                 unsigned char   EndVertRetrace;   /* CRT-Controller:11h */
  56                 unsigned char   ModeControl;      /* CRT-Controller:17h */
  57                 unsigned char   ClockingMode;     /* Seq-Controller:01h */
  58         } vga_state;
  59         struct vgastate state;
  60         unsigned int ref_count;
  61         int palette_blanked, vesa_blanked, mode, isVGA;
  62         u8 misc, pel_msk, vss, clkdiv;
  63         u8 crtc[VGA_CRT_C];
  64 };
  65
  66 /* --------------------------------------------------------------------- */
  67
  68 static struct fb_var_screeninfo vga16fb_defined = {
  69         .xres           = 640,
  70         .yres           = 480,
  71         .xres_virtual   = 640,
  72         .yres_virtual   = 480,
  73         .bits_per_pixel = 4,
  74         .activate       = FB_ACTIVATE_TEST,
  75         .height         = -1,
  76         .width          = -1,
  77         .pixclock       = 39721,
  78         .left_margin    = 48,
  79         .right_margin   = 16,
  80         .upper_margin   = 33,
  81         .lower_margin   = 10,
  82         .hsync_len      = 96,
  83         .vsync_len      = 2,
  84         .vmode          = FB_VMODE_NONINTERLACED,
  85 };
  86
  87 /* name should not depend on EGA/VGA */
  88 static const struct fb_fix_screeninfo vga16fb_fix = {
  89         .id             = "VGA16 VGA",
  90         .smem_start     = VGA_FB_PHYS,
  91         .smem_len       = VGA_FB_PHYS_LEN,
  92         .type           = FB_TYPE_VGA_PLANES,
  93         .type_aux       = FB_AUX_VGA_PLANES_VGA4,
  94         .visual         = FB_VISUAL_PSEUDOCOLOR,
  95         .xpanstep       = 8,
  96         .ypanstep       = 1,
  97         .line_length    = 640 / 8,
  98         .accel          = FB_ACCEL_NONE
  99 };
 100
 101 /* The VGA's weird architecture often requires that we read a byte and
 102    write a byte to the same location.  It doesn't matter *what* byte
 103    we write, however.  This is because all the action goes on behind
 104    the scenes in the VGA's 32-bit latch register, and reading and writing
 105    video memory just invokes latch behavior.
 106
 107    To avoid race conditions (is this necessary?), reading and writing
 108    the memory byte should be done with a single instruction.  One
 109    suitable instruction is the x86 bitwise OR.  The following
 110    read-modify-write routine should optimize to one such bitwise
 111    OR. */
 112 static inline void rmw(volatile char __iomem *p)
 113 {
 114         readb(p);
 115         writeb(1, p);
 116 }
 117
 118 /* Set the Graphics Mode Register, and return its previous value.
 119    Bits 0-1 are write mode, bit 3 is read mode. */
 120 static inline int setmode(int mode)
 121 {
 122         int oldmode;
 123
 124         oldmode = vga_io_rgfx(VGA_GFX_MODE);
 125         vga_io_w(VGA_GFX_D, mode);
 126         return oldmode;
 127 }
 128
 129 /* Select the Bit Mask Register and return its value. */
 130 static inline int selectmask(void)
 131 {
 132         return vga_io_rgfx(VGA_GFX_BIT_MASK);
 133 }
 134
 135 /* Set the value of the Bit Mask Register.  It must already have been
 136    selected with selectmask(). */
 137 static inline void setmask(int mask)
 138 {
 139         vga_io_w(VGA_GFX_D, mask);
 140 }
 141
 142 /* Set the Data Rotate Register and return its old value.
 143    Bits 0-2 are rotate count, bits 3-4 are logical operation
 144    (0=NOP, 1=AND, 2=OR, 3=XOR). */
 145 static inline int setop(int op)
 146 {
 147         int oldop;
 148
 149         oldop = vga_io_rgfx(VGA_GFX_DATA_ROTATE);
 150         vga_io_w(VGA_GFX_D, op);
 151         return oldop;
 152 }
 153
 154 /* Set the Enable Set/Reset Register and return its old value.
 155    The code here always uses value 0xf for this register. */
 156 static inline int setsr(int sr)
 157 {
 158         int oldsr;
 159
 160         oldsr = vga_io_rgfx(VGA_GFX_SR_ENABLE);
 161         vga_io_w(VGA_GFX_D, sr);
 162         return oldsr;
 163 }
 164
 165 /* Set the Set/Reset Register and return its old value. */
 166 static inline int setcolor(int color)
 167 {
 168         int oldcolor;
 169
 170         oldcolor = vga_io_rgfx(VGA_GFX_SR_VALUE);
 171         vga_io_w(VGA_GFX_D, color);
 172         return oldcolor;
 173 }
 174
 175 /* Return the value in the Graphics Address Register. */
 176 static inline int getindex(void)
 177 {
 178         return vga_io_r(VGA_GFX_I);
 179 }
 180
 181 /* Set the value in the Graphics Address Register. */
 182 static inline void setindex(int index)
 183 {
 184         vga_io_w(VGA_GFX_I, index);
 185 }
 186
 187 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                         line_ofs = info->fix.line_length - width;
1059
1060                         setmode(1);
1061                         setop(0);
1062                         setsr(0xf);
1063
1064                         if (dy < sy || (dy == sy && dx < sx)) {
1065                                 dst = info->screen_base + (dx/8) + dy * info->fix.line_length;
1066                                 src = info->screen_base + (sx/8) + sy * info->fix.line_length;
1067                                 while (height--) {
1068                                         for (x = 0; x < width; x++) {
1069                                                 readb(src);
1070                                                 writeb(0, dst);
1071                                                 dst++;
1072                                                 src++;
1073                                         }
1074                                         src += line_ofs;
1075                                         dst += line_ofs;
1076                                 }
1077                         } else {
1078                                 dst = info->screen_base + (dx/8) + width +
1079                                         (dy + height - 1) * info->fix.line_length;
1080                                 src = info->screen_base + (sx/8) + width +
1081                                         (sy + height  - 1) * info->fix.line_length;
1082                                 while (height--) {
1083                                         for (x = 0; x < width; x++) {
1084                                                 dst--;
1085                                                 src--;
1086                                                 readb(src);
1087                                                 writeb(0, dst);
1088                                         }
1089                                         src -= line_ofs;
1090                                         dst -= line_ofs;
1091                                 }
1092                         }
1093                 } else
1094                         vga_8planes_copyarea(info, area);
1095                 break;
1096         case FB_TYPE_PACKED_PIXELS:
1097         default:
1098                 cfb_copyarea(info, area);
1099                 break;
1100         }
1101 }
1102
1103 #define TRANS_MASK_LOW  {0x0,0x8,0x4,0xC,0x2,0xA,0x6,0xE,0x1,0x9,0x5,0xD,0x3,0xB,0x7,0xF}
1104 #define TRANS_MASK_HIGH {0x000, 0x800, 0x400, 0xC00, 0x200, 0xA00, 0x600, 0xE00, \
1105                          0x100, 0x900, 0x500, 0xD00, 0x300, 0xB00, 0x700, 0xF00}
1106
1107 #if defined(__LITTLE_ENDIAN)
1108 static const u16 transl_l[] = TRANS_MASK_LOW;
1109 static const u16 transl_h[] = TRANS_MASK_HIGH;
1110 #elif defined(__BIG_ENDIAN)
1111 static const u16 transl_l[] = TRANS_MASK_HIGH;
1112 static const u16 transl_h[] = TRANS_MASK_LOW;
1113 #else
1114 #error "Only __BIG_ENDIAN and __LITTLE_ENDIAN are supported in vga-planes"
1115 #endif
1116
1117 static void vga_8planes_imageblit(struct fb_info *info, const struct fb_image *image)
1118 {
1119         char oldindex = getindex();
1120         char oldmode = setmode(0x40);
1121         char oldop = setop(0);
1122         char oldsr = setsr(0);
1123         char oldmask = selectmask();
1124         const char *cdat = image->data;
1125         u32 dx = image->dx;
1126         char __iomem *where;
1127         int y;
1128
1129         dx /= 4;
1130         where = info->screen_base + dx + image->dy * info->fix.line_length;
1131
1132         setmask(0xff);
1133         writeb(image->bg_color, where);
1134         readb(where);
1135         selectmask();
1136         setmask(image->fg_color ^ image->bg_color);
1137         setmode(0x42);
1138         setop(0x18);
1139         for (y = 0; y < image->height; y++, where += info->fix.line_length)
1140                 writew(transl_h[cdat[y]&0xF] | transl_l[cdat[y] >> 4], where);
1141         setmask(oldmask);
1142         setsr(oldsr);
1143         setop(oldop);
1144         setmode(oldmode);
1145         setindex(oldindex);
1146 }
1147
1148 static void vga_imageblit_expand(struct fb_info *info, const struct fb_image *image)
1149 {
1150         char __iomem *where = info->screen_base + (image->dx/8) +
1151                 image->dy * info->fix.line_length;
1152         struct vga16fb_par *par = info->par;
1153         char *cdat = (char *) image->data;
1154         char __iomem *dst;
1155         int x, y;
1156
1157         switch (info->fix.type) {
1158         case FB_TYPE_VGA_PLANES:
1159                 if (info->fix.type_aux == FB_AUX_VGA_PLANES_VGA4) {
1160                         if (par->isVGA) {
1161                                 setmode(2);
1162                                 setop(0);
1163                                 setsr(0xf);
1164                                 setcolor(image->fg_color);
1165                                 selectmask();
1166
1167                                 setmask(0xff);
1168                                 writeb(image->bg_color, where);
1169                                 rmb();
1170                                 readb(where); /* fill latches */
1171                                 setmode(3);
1172                                 wmb();
1173                                 for (y = 0; y < image->height; y++) {
1174                                         dst = where;
1175                                         for (x = image->width/8; x--;)
1176                                                 writeb(*cdat++, dst++);
1177                                         where += info->fix.line_length;
1178                                 }
1179                                 wmb();
1180                         } else {
1181                                 setmode(0);
1182                                 setop(0);
1183                                 setsr(0xf);
1184                                 setcolor(image->bg_color);
1185                                 selectmask();
1186
1187                                 setmask(0xff);
1188                                 for (y = 0; y < image->height; y++) {
1189                                         dst = where;
1190                                         for (x=image->width/8; x--;){
1191                                                 rmw(dst);
1192                                                 setcolor(image->fg_color);
1193                                                 selectmask();
1194                                                 if (*cdat) {
1195                                                         setmask(*cdat++);
1196                                                         rmw(dst++);
1197                                                 }
1198                                         }
1199                                         where += info->fix.line_length;
1200                                 }
1201                         }
1202                 } else
1203                         vga_8planes_imageblit(info, image);
1204                 break;
1205         case FB_TYPE_PACKED_PIXELS:
1206         default:
1207                 cfb_imageblit(info, image);
1208                 break;
1209         }
1210 }
1211
1212 static void vga_imageblit_color(struct fb_info *info, const struct fb_image *image)
1213 {
1214         /*
1215          * Draw logo
1216          */
1217         struct vga16fb_par *par = info->par;
1218         char __iomem *where =
1219                 info->screen_base + image->dy * info->fix.line_length +
1220                 image->dx/8;
1221         const char *cdat = image->data;
1222         char __iomem *dst;
1223         int x, y;
1224
1225         switch (info->fix.type) {
1226         case FB_TYPE_VGA_PLANES:
1227                 if (info->fix.type_aux == FB_AUX_VGA_PLANES_VGA4 &&
1228                     par->isVGA) {
1229                         setsr(0xf);
1230                         setop(0);
1231                         setmode(0);
1232
1233                         for (y = 0; y < image->height; y++) {
1234                                 for (x = 0; x < image->width; x++) {
1235                                         dst = where + x/8;
1236
1237                                         setcolor(*cdat);
1238                                         selectmask();
1239                                         setmask(1 << (7 - (x % 8)));
1240                                         fb_readb(dst);
1241                                         fb_writeb(0, dst);
1242
1243                                         cdat++;
1244                                 }
1245                                 where += info->fix.line_length;
1246                         }
1247                 }
1248                 break;
1249         case FB_TYPE_PACKED_PIXELS:
1250                 cfb_imageblit(info, image);
1251                 break;
1252         default:
1253                 break;
1254         }
1255 }
1256
1257 static void vga16fb_imageblit(struct fb_info *info, const struct fb_image *image)
1258 {
1259         if (image->depth == 1)
1260                 vga_imageblit_expand(info, image);
1261         else
1262                 vga_imageblit_color(info, image);
1263 }
1264
1265 static void vga16fb_destroy(struct fb_info *info)
1266 {
1267         iounmap(info->screen_base);
1268         fb_dealloc_cmap(&info->cmap);
1269         /* XXX unshare VGA regions */
1270         framebuffer_release(info);
1271 }
1272
1273 static struct fb_ops vga16fb_ops = {
1274         .owner          = THIS_MODULE,
1275         .fb_open        = vga16fb_open,
1276         .fb_release     = vga16fb_release,
1277         .fb_destroy     = vga16fb_destroy,
1278         .fb_check_var   = vga16fb_check_var,
1279         .fb_set_par     = vga16fb_set_par,
1280         .fb_setcolreg   = vga16fb_setcolreg,
1281         .fb_pan_display = vga16fb_pan_display,
1282         .fb_blank       = vga16fb_blank,
1283         .fb_fillrect    = vga16fb_fillrect,
1284         .fb_copyarea    = vga16fb_copyarea,
1285         .fb_imageblit   = vga16fb_imageblit,
1286 };
1287
1288 #ifndef MODULE
1289 static int __init vga16fb_setup(char *options)
1290 {
1291         char *this_opt;
1292
1293         if (!options || !*options)
1294                 return 0;
1295
1296         while ((this_opt = strsep(&options, ",")) != NULL) {
1297                 if (!*this_opt) continue;
1298         }
1299         return 0;
1300 }
1301 #endif
1302
1303 static int vga16fb_probe(struct platform_device *dev)
1304 {
1305         struct fb_info *info;
1306         struct vga16fb_par *par;
1307         int i;
1308         int ret = 0;
1309
1310         printk(KERN_DEBUG "vga16fb: initializing\n");
1311         info = framebuffer_alloc(sizeof(struct vga16fb_par), &dev->dev);
1312
1313         if (!info) {
1314                 ret = -ENOMEM;
1315                 goto err_fb_alloc;
1316         }
1317         info->apertures = alloc_apertures(1);
1318         if (!info->apertures) {
1319                 ret = -ENOMEM;
1320                 goto err_ioremap;
1321         }
1322
1323         /* XXX share VGA_FB_PHYS and I/O region with vgacon and others */
1324         info->screen_base = (void __iomem *)VGA_MAP_MEM(VGA_FB_PHYS, 0);
1325
1326         if (!info->screen_base) {
1327                 printk(KERN_ERR "vga16fb: unable to map device\n");
1328                 ret = -ENOMEM;
1329                 goto err_ioremap;
1330         }
1331
1332         printk(KERN_INFO "vga16fb: mapped to 0x%p\n", info->screen_base);
1333         par = info->par;
1334
1335         par->isVGA = screen_info.orig_video_isVGA;
1336         par->palette_blanked = 0;
1337         par->vesa_blanked = 0;
1338
1339         i = par->isVGA? 6 : 2;
1340
1341         vga16fb_defined.red.length   = i;
1342         vga16fb_defined.green.length = i;
1343         vga16fb_defined.blue.length  = i;
1344
1345         /* name should not depend on EGA/VGA */
1346         info->fbops = &vga16fb_ops;
1347         info->var = vga16fb_defined;
1348         info->fix = vga16fb_fix;
1349         /* supports rectangles with widths of multiples of 8 */
1350         info->pixmap.blit_x = 1 << 7 | 1 << 15 | 1 << 23 | 1 << 31;
1351         info->flags = FBINFO_FLAG_DEFAULT | FBINFO_MISC_FIRMWARE |
1352                 FBINFO_HWACCEL_YPAN;
1353
1354         i = (info->var.bits_per_pixel == 8) ? 256 : 16;
1355         ret = fb_alloc_cmap(&info->cmap, i, 0);
1356         if (ret) {
1357                 printk(KERN_ERR "vga16fb: unable to allocate colormap\n");
1358                 ret = -ENOMEM;
1359                 goto err_alloc_cmap;
1360         }
1361
1362         if (vga16fb_check_var(&info->var, info)) {
1363                 printk(KERN_ERR "vga16fb: unable to validate variable\n");
1364                 ret = -EINVAL;
1365                 goto err_check_var;
1366         }
1367
1368         vga16fb_update_fix(info);
1369
1370         info->apertures->ranges[0].base = VGA_FB_PHYS;
1371         info->apertures->ranges[0].size = VGA_FB_PHYS_LEN;
1372
1373         if (register_framebuffer(info) < 0) {
1374                 printk(KERN_ERR "vga16fb: unable to register framebuffer\n");
1375                 ret = -EINVAL;
1376                 goto err_check_var;
1377         }
1378
1379         fb_info(info, "%s frame buffer device\n", info->fix.id);
1380         platform_set_drvdata(dev, info);
1381
1382         return 0;
1383
1384  err_check_var:
1385         fb_dealloc_cmap(&info->cmap);
1386  err_alloc_cmap:
1387         iounmap(info->screen_base);
1388  err_ioremap:
1389         framebuffer_release(info);
1390  err_fb_alloc:
1391         return ret;
1392 }
1393
1394 static int vga16fb_remove(struct platform_device *dev)
1395 {
1396         struct fb_info *info = platform_get_drvdata(dev);
1397
1398         if (info)
1399                 unregister_framebuffer(info);
1400
1401         return 0;
1402 }
1403
1404 static struct platform_driver vga16fb_driver = {
1405         .probe = vga16fb_probe,
1406         .remove = vga16fb_remove,
1407         .driver = {
1408                 .name = "vga16fb",
1409         },
1410 };
1411
1412 static struct platform_device *vga16fb_device;
1413
1414 static int __init vga16fb_init(void)
1415 {
1416         int ret;
1417 #ifndef MODULE
1418         char *option = NULL;
1419
1420         if (fb_get_options("vga16fb", &option))
1421                 return -ENODEV;
1422
1423         vga16fb_setup(option);
1424 #endif
1425         ret = platform_driver_register(&vga16fb_driver);
1426
1427         if (!ret) {
1428                 vga16fb_device = platform_device_alloc("vga16fb", 0);
1429
1430                 if (vga16fb_device)
1431                         ret = platform_device_add(vga16fb_device);
1432                 else
1433                         ret = -ENOMEM;
1434
1435                 if (ret) {
1436                         platform_device_put(vga16fb_device);
1437                         platform_driver_unregister(&vga16fb_driver);
1438                 }
1439         }
1440
1441         return ret;
1442 }
1443
1444 static void __exit vga16fb_exit(void)
1445 {
1446         platform_device_unregister(vga16fb_device);
1447         platform_driver_unregister(&vga16fb_driver);
1448 }
1449
1450 MODULE_DESCRIPTION("Legacy VGA framebuffer device driver");
1451 MODULE_LICENSE("GPL");
1452 module_init(vga16fb_init);
1453 module_exit(vga16fb_exit);
1454
1455
1456 /*
1457  * Overrides for Emacs so that we follow Linus's tabbing style.
1458  * ---------------------------------------------------------------------------
1459  * Local variables:
1460  * c-basic-offset: 8
1461  * End:
1462  */
1463