SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/video/vga16fb.c

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