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