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