drivers/video/vga16fb.c

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