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