drivers/video/vga16fb.c

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