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