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