SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/video/vga16fb.c

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