git.oblomov.eu Git - linux-2.6/blob - drivers/video/vga16fb.c

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