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