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/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
40
41
42struct vga16fb_par {
43
44
45 struct {
46 unsigned char SeqCtrlIndex;
47 unsigned char CrtCtrlIndex;
48 unsigned char CrtMiscIO;
49 unsigned char HorizontalTotal;
50 unsigned char HorizDisplayEnd;
51 unsigned char StartHorizRetrace;
52 unsigned char EndHorizRetrace;
53 unsigned char Overflow;
54 unsigned char StartVertRetrace;
55 unsigned char EndVertRetrace;
56 unsigned char ModeControl;
57 unsigned char ClockingMode;
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 = {
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
88static struct fb_fix_screeninfo vga16fb_fix = {
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
102
103
104
105
106
107
108
109
110
111
112static inline void rmw(volatile char __iomem *p)
113{
114 readb(p);
115 writeb(1, p);
116}
117
118
119
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
130static inline int selectmask(void)
131{
132 return vga_io_rgfx(VGA_GFX_BIT_MASK);
133}
134
135
136
137static inline void setmask(int mask)
138{
139 vga_io_w(VGA_GFX_D, mask);
140}
141
142
143
144
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
155
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
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
176static inline int getindex(void)
177{
178 return vga_io_r(VGA_GFX_I);
179}
180
181
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;
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
207
208 vga_io_r(VGA_IS1_RC);
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 {
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 , 0x00, 0x08},
256 { 70616 , 0x04, 0x08},
257 { 39721 , 0x00, 0x00},
258 { 35308 , 0x04, 0x00},
259 { 0 , 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;
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;
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;
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;
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;
462 if (pos & 0x200) {
463 r7 |= 0x40;
464 par->crtc[VGA_CRTC_MAX_SCAN] |= 0x20;
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;
474 pos += upper - 1;
475 par->crtc[VGA_CRTC_V_BLANK_END] = pos & 0xFF;
476
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;
482 else
483 par->crtc[VGA_CRTC_UNDERLINE] = 0x1F;
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;
489
490 par->misc = 0xE3;
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
500 vga16fb_clock_chip(par, var->pixclock, info, 1, 2);
501 else
502
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;
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;
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
582 if (!par->isVGA) {
583 vga_io_w(EGA_GFX_E0, 0x00);
584 vga_io_w(EGA_GFX_E1, 0x01);
585 }
586
587
588 vga_io_w(VGA_MIS_W, par->misc);
589
590
591 vga_io_wseq(0x00, 0x01);
592
593 if (par->isVGA)
594 vga_io_w(VGA_PEL_MSK, par->pel_msk);
595
596
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
603 vga_io_wseq(0x00, 0x03);
604
605
606 vga_io_wcrt(VGA_CRTC_V_SYNC_END, par->crtc[VGA_CRTC_V_SYNC_END]);
607
608
609 for (i = 0; i < VGA_CRTC_REGS; i++) {
610 vga_io_wcrt(i, par->crtc[i]);
611 }
612
613
614 for (i = 0; i < VGA_GFX_C; i++) {
615 vga_io_wgfx(i, gdc[i]);
616 }
617
618
619 for (i = 0; i < VGA_ATT_C; i++) {
620 vga_io_r(VGA_IS1_RC);
621 vga_io_wattr(i, atc[i]);
622 }
623
624
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);
645 vga_io_wattr(regno, val);
646 vga_io_r(VGA_IS1_RC);
647 vga_io_w(VGA_ATT_IW, 0x20);
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
667
668
669
670
671
672 if (regno >= 256)
673 return 1;
674
675 gray = info->var.grayscale;
676
677 if (gray) {
678
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
696
697
698
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
706 if(!par->vesa_blanked) {
707 par->vga_state.CrtMiscIO = vga_io_r(VGA_MIS_R);
708
709
710 par->vga_state.HorizontalTotal = vga_io_rcrt(0x00);
711 par->vga_state.HorizDisplayEnd = vga_io_rcrt(0x01);
712 par->vga_state.StartHorizRetrace = vga_io_rcrt(0x04);
713 par->vga_state.EndHorizRetrace = vga_io_rcrt(0x05);
714 par->vga_state.Overflow = vga_io_rcrt(0x07);
715 par->vga_state.StartVertRetrace = vga_io_rcrt(0x10);
716 par->vga_state.EndVertRetrace = vga_io_rcrt(0x11);
717 par->vga_state.ModeControl = vga_io_rcrt(0x17);
718 par->vga_state.ClockingMode = vga_io_rseq(0x01);
719 }
720
721
722
723 vga_io_wseq(0x01, par->vga_state.ClockingMode | 0x20);
724
725
726 if ((par->vga_state.CrtMiscIO & 0x80) == 0x80)
727 vga_io_w(VGA_MIS_W, par->vga_state.CrtMiscIO & 0xef);
728
729
730
731
732
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
738 vga_io_wcrt(VGA_CRTC_OVERFLOW, par->vga_state.Overflow | 0x84);
739 }
740
741 if (mode & FB_BLANK_HSYNC_SUSPEND) {
742
743
744
745
746
747 vga_io_wcrt(VGA_CRTC_H_SYNC_START, 0xff);
748 vga_io_wcrt(VGA_CRTC_H_SYNC_END, 0x00);
749 }
750
751
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
762 vga_io_w(VGA_MIS_W, par->vga_state.CrtMiscIO);
763
764
765 vga_io_wcrt(0x00, par->vga_state.HorizontalTotal);
766
767 vga_io_wcrt(0x01, par->vga_state.HorizDisplayEnd);
768
769 vga_io_wcrt(0x04, par->vga_state.StartHorizRetrace);
770
771 vga_io_wcrt(0x05, par->vga_state.EndHorizRetrace);
772
773 vga_io_wcrt(0x07, par->vga_state.Overflow);
774
775 vga_io_wcrt(0x10, par->vga_state.StartVertRetrace);
776
777 vga_io_wcrt(0x11, par->vga_state.EndVertRetrace);
778
779 vga_io_wcrt(0x17, par->vga_state.ModeControl);
780
781 vga_io_wseq(0x01, par->vga_state.ClockingMode);
782
783
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
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:
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:
816 vga_pal_blank();
817 par->palette_blanked = 1;
818 break;
819 default:
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
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
893
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
1027 old_dx = area->dx;
1028 old_dy = area->dy;
1029
1030
1031
1032
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
1047 sx += (dx - old_dx);
1048 sy += (dy - old_dy);
1049
1050
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);
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
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 iounmap(info->screen_base);
1269 fb_dealloc_cmap(&info->cmap);
1270
1271 framebuffer_release(info);
1272}
1273
1274static struct fb_ops vga16fb_ops = {
1275 .owner = THIS_MODULE,
1276 .fb_open = vga16fb_open,
1277 .fb_release = vga16fb_release,
1278 .fb_destroy = vga16fb_destroy,
1279 .fb_check_var = vga16fb_check_var,
1280 .fb_set_par = vga16fb_set_par,
1281 .fb_setcolreg = vga16fb_setcolreg,
1282 .fb_pan_display = vga16fb_pan_display,
1283 .fb_blank = vga16fb_blank,
1284 .fb_fillrect = vga16fb_fillrect,
1285 .fb_copyarea = vga16fb_copyarea,
1286 .fb_imageblit = vga16fb_imageblit,
1287};
1288
1289#ifndef MODULE
1290static int __init vga16fb_setup(char *options)
1291{
1292 char *this_opt;
1293
1294 if (!options || !*options)
1295 return 0;
1296
1297 while ((this_opt = strsep(&options, ",")) != NULL) {
1298 if (!*this_opt) continue;
1299 }
1300 return 0;
1301}
1302#endif
1303
1304static int vga16fb_probe(struct platform_device *dev)
1305{
1306 struct fb_info *info;
1307 struct vga16fb_par *par;
1308 int i;
1309 int ret = 0;
1310
1311 printk(KERN_DEBUG "vga16fb: initializing\n");
1312 info = framebuffer_alloc(sizeof(struct vga16fb_par), &dev->dev);
1313
1314 if (!info) {
1315 ret = -ENOMEM;
1316 goto err_fb_alloc;
1317 }
1318 info->apertures = alloc_apertures(1);
1319 if (!info->apertures) {
1320 ret = -ENOMEM;
1321 goto err_ioremap;
1322 }
1323
1324
1325 info->screen_base = (void __iomem *)VGA_MAP_MEM(VGA_FB_PHYS, 0);
1326
1327 if (!info->screen_base) {
1328 printk(KERN_ERR "vga16fb: unable to map device\n");
1329 ret = -ENOMEM;
1330 goto err_ioremap;
1331 }
1332
1333 printk(KERN_INFO "vga16fb: mapped to 0x%p\n", info->screen_base);
1334 par = info->par;
1335
1336 par->isVGA = screen_info.orig_video_isVGA;
1337 par->palette_blanked = 0;
1338 par->vesa_blanked = 0;
1339
1340 i = par->isVGA? 6 : 2;
1341
1342 vga16fb_defined.red.length = i;
1343 vga16fb_defined.green.length = i;
1344 vga16fb_defined.blue.length = i;
1345
1346
1347 info->fbops = &vga16fb_ops;
1348 info->var = vga16fb_defined;
1349 info->fix = vga16fb_fix;
1350
1351 info->pixmap.blit_x = 1 << 7 | 1 << 15 | 1 << 23 | 1 << 31;
1352 info->flags = FBINFO_FLAG_DEFAULT | FBINFO_MISC_FIRMWARE |
1353 FBINFO_HWACCEL_YPAN;
1354
1355 i = (info->var.bits_per_pixel == 8) ? 256 : 16;
1356 ret = fb_alloc_cmap(&info->cmap, i, 0);
1357 if (ret) {
1358 printk(KERN_ERR "vga16fb: unable to allocate colormap\n");
1359 ret = -ENOMEM;
1360 goto err_alloc_cmap;
1361 }
1362
1363 if (vga16fb_check_var(&info->var, info)) {
1364 printk(KERN_ERR "vga16fb: unable to validate variable\n");
1365 ret = -EINVAL;
1366 goto err_check_var;
1367 }
1368
1369 vga16fb_update_fix(info);
1370
1371 info->apertures->ranges[0].base = VGA_FB_PHYS;
1372 info->apertures->ranges[0].size = VGA_FB_PHYS_LEN;
1373
1374 if (register_framebuffer(info) < 0) {
1375 printk(KERN_ERR "vga16fb: unable to register framebuffer\n");
1376 ret = -EINVAL;
1377 goto err_check_var;
1378 }
1379
1380 fb_info(info, "%s frame buffer device\n", info->fix.id);
1381 platform_set_drvdata(dev, info);
1382
1383 return 0;
1384
1385 err_check_var:
1386 fb_dealloc_cmap(&info->cmap);
1387 err_alloc_cmap:
1388 iounmap(info->screen_base);
1389 err_ioremap:
1390 framebuffer_release(info);
1391 err_fb_alloc:
1392 return ret;
1393}
1394
1395static int vga16fb_remove(struct platform_device *dev)
1396{
1397 struct fb_info *info = platform_get_drvdata(dev);
1398
1399 if (info)
1400 unregister_framebuffer(info);
1401
1402 return 0;
1403}
1404
1405static struct platform_driver vga16fb_driver = {
1406 .probe = vga16fb_probe,
1407 .remove = vga16fb_remove,
1408 .driver = {
1409 .name = "vga16fb",
1410 },
1411};
1412
1413static struct platform_device *vga16fb_device;
1414
1415static int __init vga16fb_init(void)
1416{
1417 int ret;
1418#ifndef MODULE
1419 char *option = NULL;
1420
1421 if (fb_get_options("vga16fb", &option))
1422 return -ENODEV;
1423
1424 vga16fb_setup(option);
1425#endif
1426 ret = platform_driver_register(&vga16fb_driver);
1427
1428 if (!ret) {
1429 vga16fb_device = platform_device_alloc("vga16fb", 0);
1430
1431 if (vga16fb_device)
1432 ret = platform_device_add(vga16fb_device);
1433 else
1434 ret = -ENOMEM;
1435
1436 if (ret) {
1437 platform_device_put(vga16fb_device);
1438 platform_driver_unregister(&vga16fb_driver);
1439 }
1440 }
1441
1442 return ret;
1443}
1444
1445static void __exit vga16fb_exit(void)
1446{
1447 platform_device_unregister(vga16fb_device);
1448 platform_driver_unregister(&vga16fb_driver);
1449}
1450
1451MODULE_DESCRIPTION("Legacy VGA framebuffer device driver");
1452MODULE_LICENSE("GPL");
1453module_init(vga16fb_init);
1454module_exit(vga16fb_exit);
1455
1456
1457
1458
1459
1460
1461
1462
1463
1464
1465
