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