1
2
3
4
5
6
7
8
9
10
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
41
42
43struct vga16fb_par {
44
45
46 struct {
47 unsigned char SeqCtrlIndex;
48 unsigned char CrtCtrlIndex;
49 unsigned char CrtMiscIO;
50 unsigned char HorizontalTotal;
51 unsigned char HorizDisplayEnd;
52 unsigned char StartHorizRetrace;
53 unsigned char EndHorizRetrace;
54 unsigned char Overflow;
55 unsigned char StartVertRetrace;
56 unsigned char EndVertRetrace;
57 unsigned char ModeControl;
58 unsigned char ClockingMode;
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
69static 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
89static 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
103
104
105
106
107
108
109
110
111
112
113static inline void rmw(volatile char __iomem *p)
114{
115 readb(p);
116 writeb(1, p);
117}
118
119
120
121static 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
131static inline int selectmask(void)
132{
133 return vga_io_rgfx(VGA_GFX_BIT_MASK);
134}
135
136
137
138static inline void setmask(int mask)
139{
140 vga_io_w(VGA_GFX_D, mask);
141}
142
143
144
145
146static 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
156
157static 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
167static 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
177static inline int getindex(void)
178{
179 return vga_io_r(VGA_GFX_I);
180}
181
182
183static inline void setindex(int index)
184{
185 vga_io_w(VGA_GFX_I, index);
186}
187
188static 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;
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
208
209 vga_io_r(VGA_IS1_RC);
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
219static 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 {
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
246static 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 , 0x00, 0x08},
257 { 70616 , 0x04, 0x08},
258 { 39721 , 0x00, 0x00},
259 { 35308 , 0x04, 0x00},
260 { 0 , 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
284static 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
299static 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
313static 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;
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;
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;
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;
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;
463 if (pos & 0x200) {
464 r7 |= 0x40;
465 par->crtc[VGA_CRTC_MAX_SCAN] |= 0x20;
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;
475 pos += upper - 1;
476 par->crtc[VGA_CRTC_V_BLANK_END] = pos & 0xFF;
477
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;
483 else
484 par->crtc[VGA_CRTC_UNDERLINE] = 0x1F;
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;
490
491 par->misc = 0xE3;
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
501 vga16fb_clock_chip(par, var->pixclock, info, 1, 2);
502 else
503
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
519static 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;
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;
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
583 if (!par->isVGA) {
584 vga_io_w(EGA_GFX_E0, 0x00);
585 vga_io_w(EGA_GFX_E1, 0x01);
586 }
587
588
589 vga_io_w(VGA_MIS_W, par->misc);
590
591
592 vga_io_wseq(0x00, 0x01);
593
594 if (par->isVGA)
595 vga_io_w(VGA_PEL_MSK, par->pel_msk);
596
597
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
604 vga_io_wseq(0x00, 0x03);
605
606
607 vga_io_wcrt(VGA_CRTC_V_SYNC_END, par->crtc[VGA_CRTC_V_SYNC_END]);
608
609
610 for (i = 0; i < VGA_CRTC_REGS; i++) {
611 vga_io_wcrt(i, par->crtc[i]);
612 }
613
614
615 for (i = 0; i < VGA_GFX_C; i++) {
616 vga_io_wgfx(i, gdc[i]);
617 }
618
619
620 for (i = 0; i < VGA_ATT_C; i++) {
621 vga_io_r(VGA_IS1_RC);
622 vga_io_wattr(i, atc[i]);
623 }
624
625
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
637static 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);
646 vga_io_wattr(regno, val);
647 vga_io_r(VGA_IS1_RC);
648 vga_io_w(VGA_ATT_IW, 0x20);
649}
650
651static 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
659static 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
668
669
670
671
672
673 if (regno >= 256)
674 return 1;
675
676 gray = info->var.grayscale;
677
678 if (gray) {
679
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
689static 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
697
698
699
700
701static 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
707 if(!par->vesa_blanked) {
708 par->vga_state.CrtMiscIO = vga_io_r(VGA_MIS_R);
709
710
711 par->vga_state.HorizontalTotal = vga_io_rcrt(0x00);
712 par->vga_state.HorizDisplayEnd = vga_io_rcrt(0x01);
713 par->vga_state.StartHorizRetrace = vga_io_rcrt(0x04);
714 par->vga_state.EndHorizRetrace = vga_io_rcrt(0x05);
715 par->vga_state.Overflow = vga_io_rcrt(0x07);
716 par->vga_state.StartVertRetrace = vga_io_rcrt(0x10);
717 par->vga_state.EndVertRetrace = vga_io_rcrt(0x11);
718 par->vga_state.ModeControl = vga_io_rcrt(0x17);
719 par->vga_state.ClockingMode = vga_io_rseq(0x01);
720 }
721
722
723
724 vga_io_wseq(0x01, par->vga_state.ClockingMode | 0x20);
725
726
727 if ((par->vga_state.CrtMiscIO & 0x80) == 0x80)
728 vga_io_w(VGA_MIS_W, par->vga_state.CrtMiscIO & 0xef);
729
730
731
732
733
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
739 vga_io_wcrt(VGA_CRTC_OVERFLOW, par->vga_state.Overflow | 0x84);
740 }
741
742 if (mode & FB_BLANK_HSYNC_SUSPEND) {
743
744
745
746
747
748 vga_io_wcrt(VGA_CRTC_H_SYNC_START, 0xff);
749 vga_io_wcrt(VGA_CRTC_H_SYNC_END, 0x00);
750 }
751
752
753 outb_p(SeqCtrlIndex, VGA_SEQ_I);
754 outb_p(CrtCtrlIndex, VGA_CRT_IC);
755}
756
757static 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
763 vga_io_w(VGA_MIS_W, par->vga_state.CrtMiscIO);
764
765
766 vga_io_wcrt(0x00, par->vga_state.HorizontalTotal);
767
768 vga_io_wcrt(0x01, par->vga_state.HorizDisplayEnd);
769
770 vga_io_wcrt(0x04, par->vga_state.StartHorizRetrace);
771
772 vga_io_wcrt(0x05, par->vga_state.EndHorizRetrace);
773
774 vga_io_wcrt(0x07, par->vga_state.Overflow);
775
776 vga_io_wcrt(0x10, par->vga_state.StartVertRetrace);
777
778 vga_io_wcrt(0x11, par->vga_state.EndVertRetrace);
779
780 vga_io_wcrt(0x17, par->vga_state.ModeControl);
781
782 vga_io_wseq(0x01, par->vga_state.ClockingMode);
783
784
785 vga_io_w(VGA_SEQ_I, SeqCtrlIndex);
786 vga_io_w(VGA_CRT_IC, CrtCtrlIndex);
787}
788
789static 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
802static 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:
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:
817 vga_pal_blank();
818 par->palette_blanked = 1;
819 break;
820 default:
821 vga_vesa_blank(par, blank);
822 par->vesa_blanked = 1;
823 break;
824 }
825 return 0;
826}
827
828static 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
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
882static 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
894
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
957static 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
1012static 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
1028 old_dx = area->dx;
1029 old_dy = area->dy;
1030
1031
1032
1033
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
1048 sx += (dx - old_dx);
1049 sy += (dy - old_dy);
1050
1051
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)
1110static const u16 transl_l[] = TRANS_MASK_LOW;
1111static const u16 transl_h[] = TRANS_MASK_HIGH;
1112#elif defined(__BIG_ENDIAN)
1113static const u16 transl_l[] = TRANS_MASK_HIGH;
1114static 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
1119static 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
1150static 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);
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
1214static void vga_imageblit_color(struct fb_info *info, const struct fb_image *image)
1215{
1216
1217
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
1259static 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
1267static 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
1282static 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
1296static 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
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
1334 info->fbops = &vga16fb_ops;
1335 info->var = vga16fb_defined;
1336 info->fix = vga16fb_fix;
1337
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
1380static 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
1389 framebuffer_release(info);
1390 }
1391
1392 return 0;
1393}
1394
1395static struct platform_driver vga16fb_driver = {
1396 .probe = vga16fb_probe,
1397 .remove = vga16fb_remove,
1398 .driver = {
1399 .name = "vga16fb",
1400 },
1401};
1402
1403static struct platform_device *vga16fb_device;
1404
1405static 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
1435static void __exit vga16fb_exit(void)
1436{
1437 platform_device_unregister(vga16fb_device);
1438 platform_driver_unregister(&vga16fb_driver);
1439}
1440
1441MODULE_DESCRIPTION("Legacy VGA framebuffer device driver");
1442MODULE_LICENSE("GPL");
1443module_init(vga16fb_init);
1444module_exit(vga16fb_exit);
1445
1446
1447
1448
1449
1450
1451
1452
1453
1454
1455
