WebSVN - shark - Blame - Rev 1056 - /shark/trunk/drivers/fb/vgastate.c

Rev	Author	Line No.	Line
468	giacomo	1	/*
		2	* linux/drivers/video/vgastate.c -- VGA state save/restore
		3	*
		4	* Copyright 2002 James Simmons
		5	*
		6	* Copyright history from vga16fb.c:
		7	* Copyright 1999 Ben Pfaff and Petr Vandrovec
		8	* Based on VGA info at http://www.goodnet.com/~tinara/FreeVGA/home.htm
		9	* Based on VESA framebuffer (c) 1998 Gerd Knorr
		10	*
		11	* This file is subject to the terms and conditions of the GNU General
		12	* Public License. See the file COPYING in the main directory of this
		13	* archive for more details.
		14	*
		15	*/
		16	#include <linuxcomp.h>
		17
		18	#include <linux/config.h>
		19	#include <linux/module.h>
		20	#include <linux/slab.h>
		21	#include <linux/fb.h>
		22	#include <linux/vmalloc.h>
		23	#include <video/vga.h>
		24
		25	struct regstate {
		26	__u8 *vga_font0;
		27	__u8 *vga_font1;
		28	__u8 *vga_text;
		29	__u8 *vga_cmap;
		30	__u8 *attr;
		31	__u8 *crtc;
		32	__u8 *gfx;
		33	__u8 *seq;
		34	__u8 misc;
		35	};
		36
		37	static inline unsigned char vga_rcrtcs(caddr_t regbase, unsigned short iobase,
		38	unsigned char reg)
		39	{
		40	vga_w(regbase, iobase + 0x4, reg);
		41	return vga_r(regbase, iobase + 0x5);
		42	}
		43
		44	static inline void vga_wcrtcs(caddr_t regbase, unsigned short iobase,
		45	unsigned char reg, unsigned char val)
		46	{
		47	vga_w(regbase, iobase + 0x4, reg);
		48	vga_w(regbase, iobase + 0x5, val);
		49	}
		50
		51	static void save_vga_text(struct vgastate *state, caddr_t fbbase)
		52	{
		53	struct regstate saved = (struct regstate ) state->vidstate;
		54	int i;
		55	u8 misc, attr10, gr4, gr5, gr6, seq1, seq2, seq4;
		56
		57	/* if in graphics mode, no need to save */
		58	attr10 = vga_rattr(state->vgabase, 0x10);
		59	if (attr10 & 1)
		60	return;
		61
		62	/* save regs */
		63	misc = vga_r(state->vgabase, VGA_MIS_R);
		64	gr4 = vga_rgfx(state->vgabase, VGA_GFX_PLANE_READ);
		65	gr5 = vga_rgfx(state->vgabase, VGA_GFX_MODE);
		66	gr6 = vga_rgfx(state->vgabase, VGA_GFX_MISC);
		67	seq2 = vga_rseq(state->vgabase, VGA_SEQ_PLANE_WRITE);
		68	seq4 = vga_rseq(state->vgabase, VGA_SEQ_MEMORY_MODE);
		69
		70	/* force graphics mode */
		71	vga_w(state->vgabase, VGA_MIS_W, misc \| 1);
		72
		73	/* blank screen */
		74	seq1 = vga_rseq(state->vgabase, VGA_SEQ_CLOCK_MODE);
		75	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
		76	vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1 \| 1 << 5);
		77	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x3);
		78
		79	/* save font at plane 2 */
		80	if (state->flags & VGA_SAVE_FONT0) {
		81	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x4);
		82	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
		83	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x2);
		84	vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
		85	vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
		86	for (i = 0; i < 4 * 8192; i++)
		87	saved->vga_font0[i] = vga_r(fbbase, i);
		88	}
		89
		90	/* save font at plane 3 */
		91	if (state->flags & VGA_SAVE_FONT1) {
		92	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x8);
		93	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
		94	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x3);
		95	vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
		96	vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
		97	for (i = 0; i < state->memsize; i++)
		98	saved->vga_font1[i] = vga_r(fbbase, i);
		99	}
		100
		101	/* save font at plane 0/1 */
		102	if (state->flags & VGA_SAVE_TEXT) {
		103	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x1);
		104	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
		105	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x0);
		106	vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
		107	vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
		108	for (i = 0; i < 8192; i++)
		109	saved->vga_text[i] = vga_r(fbbase, i);
		110
		111	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x2);
		112	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
		113	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x1);
		114	vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
		115	vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
		116	for (i = 0; i < 8192; i++)
		117	saved->vga_text[8192+i] = vga_r(fbbase + 2 * 8192, i);
		118	}
		119
		120	/* restore regs */
		121	vga_wattr(state->vgabase, 0x10, attr10);
		122
		123	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, seq2);
		124	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, seq4);
		125
		126	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, gr4);
		127	vga_wgfx(state->vgabase, VGA_GFX_MODE, gr5);
		128	vga_wgfx(state->vgabase, VGA_GFX_MISC, gr6);
		129	vga_w(state->vgabase, VGA_MIS_W, misc);
		130
		131	/* unblank screen */
		132	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
		133	vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1 & ~(1 << 5));
		134	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x3);
		135
		136	vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1);
		137	}
		138
		139	static void restore_vga_text(struct vgastate *state, caddr_t fbbase)
		140	{
		141	struct regstate saved = (struct regstate ) state->vidstate;
		142	int i;
		143	u8 misc, gr1, gr3, gr4, gr5, gr6, gr8;
		144	u8 seq1, seq2, seq4;
		145
		146	/* save regs */
		147	misc = vga_r(state->vgabase, VGA_MIS_R);
		148	gr1 = vga_rgfx(state->vgabase, VGA_GFX_SR_ENABLE);
		149	gr3 = vga_rgfx(state->vgabase, VGA_GFX_DATA_ROTATE);
		150	gr4 = vga_rgfx(state->vgabase, VGA_GFX_PLANE_READ);
		151	gr5 = vga_rgfx(state->vgabase, VGA_GFX_MODE);
		152	gr6 = vga_rgfx(state->vgabase, VGA_GFX_MISC);
		153	gr8 = vga_rgfx(state->vgabase, VGA_GFX_BIT_MASK);
		154	seq2 = vga_rseq(state->vgabase, VGA_SEQ_PLANE_WRITE);
		155	seq4 = vga_rseq(state->vgabase, VGA_SEQ_MEMORY_MODE);
		156
		157	/* force graphics mode */
		158	vga_w(state->vgabase, VGA_MIS_W, misc \| 1);
		159
		160	/* blank screen */
		161	seq1 = vga_rseq(state->vgabase, VGA_SEQ_CLOCK_MODE);
		162	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
		163	vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1 \| 1 << 5);
		164	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x3);
		165
		166	if (state->depth == 4) {
		167	vga_wgfx(state->vgabase, VGA_GFX_DATA_ROTATE, 0x0);
		168	vga_wgfx(state->vgabase, VGA_GFX_BIT_MASK, 0xff);
		169	vga_wgfx(state->vgabase, VGA_GFX_SR_ENABLE, 0x00);
		170	}
		171
		172	/* restore font at plane 2 */
		173	if (state->flags & VGA_SAVE_FONT0) {
		174	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x4);
		175	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
		176	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x2);
		177	vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
		178	vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
		179	for (i = 0; i < 4 * 8192; i++)
		180	vga_w(fbbase, i, saved->vga_font0[i]);
		181	}
		182
		183	/* restore font at plane 3 */
		184	if (state->flags & VGA_SAVE_FONT1) {
		185	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x8);
		186	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
		187	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x3);
		188	vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
		189	vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
		190	for (i = 0; i < state->memsize; i++)
		191	vga_w(fbbase, i, saved->vga_font1[i]);
		192	}
		193
		194	/* restore font at plane 0/1 */
		195	if (state->flags & VGA_SAVE_TEXT) {
		196	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x1);
		197	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
		198	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x0);
		199	vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
		200	vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
		201	for (i = 0; i < 8192; i++)
		202	vga_w(fbbase, i, saved->vga_text[i]);
		203
		204	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x2);
		205	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
		206	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x1);
		207	vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
		208	vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
		209	for (i = 0; i < 8192; i++)
		210	vga_w(fbbase, i, saved->vga_text[8192+i]);
		211	}
		212
		213	/* unblank screen */
		214	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
		215	vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1 & ~(1 << 5));
		216	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x3);
		217
		218	/* restore regs */
		219	vga_w(state->vgabase, VGA_MIS_W, misc);
		220
		221	vga_wgfx(state->vgabase, VGA_GFX_SR_ENABLE, gr1);
		222	vga_wgfx(state->vgabase, VGA_GFX_DATA_ROTATE, gr3);
		223	vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, gr4);
		224	vga_wgfx(state->vgabase, VGA_GFX_MODE, gr5);
		225	vga_wgfx(state->vgabase, VGA_GFX_MISC, gr6);
		226	vga_wgfx(state->vgabase, VGA_GFX_BIT_MASK, gr8);
		227
		228	vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1);
		229	vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, seq2);
		230	vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, seq4);
		231	}
		232
		233	static void save_vga_mode(struct vgastate *state)
		234	{
		235	struct regstate saved = (struct regstate ) state->vidstate;
		236	unsigned short iobase;
		237	int i;
		238
		239	saved->misc = vga_r(state->vgabase, VGA_MIS_R);
		240	if (saved->misc & 1)
		241	iobase = 0x3d0;
		242	else
		243	iobase = 0x3b0;
		244
		245	for (i = 0; i < state->num_crtc; i++)
		246	saved->crtc[i] = vga_rcrtcs(state->vgabase, iobase, i);
		247
		248	vga_r(state->vgabase, iobase + 0xa);
		249	vga_w(state->vgabase, VGA_ATT_W, 0x00);
		250	for (i = 0; i < state->num_attr; i++) {
		251	vga_r(state->vgabase, iobase + 0xa);
		252	saved->attr[i] = vga_rattr(state->vgabase, i);
		253	}
		254	vga_r(state->vgabase, iobase + 0xa);
		255	vga_w(state->vgabase, VGA_ATT_W, 0x20);
		256
		257	for (i = 0; i < state->num_gfx; i++)
		258	saved->gfx[i] = vga_rgfx(state->vgabase, i);
		259
		260	for (i = 0; i < state->num_seq; i++)
		261	saved->seq[i] = vga_rseq(state->vgabase, i);
		262	}
		263
		264	static void restore_vga_mode(struct vgastate *state)
		265	{
		266	struct regstate saved = (struct regstate ) state->vidstate;
		267	unsigned short iobase;
		268	int i;
		269
		270	vga_w(state->vgabase, VGA_MIS_W, saved->misc);
		271
		272	if (saved->misc & 1)
		273	iobase = 0x3d0;
		274	else
		275	iobase = 0x3b0;
		276
		277	/* turn off display */
		278	vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE,
		279	saved->seq[VGA_SEQ_CLOCK_MODE] \| 0x20);
		280
		281	/* disable sequencer */
		282	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x01);
		283
		284	/* enable palette addressing */
		285	vga_r(state->vgabase, iobase + 0xa);
		286	vga_w(state->vgabase, VGA_ATT_W, 0x00);
		287
		288	for (i = 2; i < state->num_seq; i++)
		289	vga_wseq(state->vgabase, i, saved->seq[i]);
		290
		291
		292	/* unprotect vga regs */
		293	vga_wcrtcs(state->vgabase, iobase, 17, saved->crtc[17] & ~0x80);
		294	for (i = 0; i < state->num_crtc; i++)
		295	vga_wcrtcs(state->vgabase, iobase, i, saved->crtc[i]);
		296
		297	for (i = 0; i < state->num_gfx; i++)
		298	vga_wgfx(state->vgabase, i, saved->gfx[i]);
		299
		300	for (i = 0; i < state->num_attr; i++) {
		301	vga_r(state->vgabase, iobase + 0xa);
		302	vga_wattr(state->vgabase, i, saved->attr[i]);
		303	}
		304
		305	/* reenable sequencer */
		306	vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x03);
		307	/* turn display on */
		308	vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE,
		309	saved->seq[VGA_SEQ_CLOCK_MODE] & ~(1 << 5));
		310
		311	/* disable video/palette source */
		312	vga_r(state->vgabase, iobase + 0xa);
		313	vga_w(state->vgabase, VGA_ATT_W, 0x20);
		314	}
		315
		316	static void save_vga_cmap(struct vgastate *state)
		317	{
		318	struct regstate saved = (struct regstate ) state->vidstate;
		319	int i;
		320
		321	vga_w(state->vgabase, VGA_PEL_MSK, 0xff);
		322
		323	/* assumes DAC is readable and writable */
		324	vga_w(state->vgabase, VGA_PEL_IR, 0x00);
		325	for (i = 0; i < 768; i++)
		326	saved->vga_cmap[i] = vga_r(state->vgabase, VGA_PEL_D);
		327	}
		328
		329	static void restore_vga_cmap(struct vgastate *state)
		330	{
		331	struct regstate saved = (struct regstate ) state->vidstate;
		332	int i;
		333
		334	vga_w(state->vgabase, VGA_PEL_MSK, 0xff);
		335
		336	/* assumes DAC is readable and writable */
		337	vga_w(state->vgabase, VGA_PEL_IW, 0x00);
		338	for (i = 0; i < 768; i++)
		339	vga_w(state->vgabase, VGA_PEL_D, saved->vga_cmap[i]);
		340	}
		341
		342	static void vga_cleanup(struct vgastate *state)
		343	{
		344	if (state->vidstate != NULL) {
		345	struct regstate saved = (struct regstate ) state->vidstate;
		346
		347	if (saved->vga_font0)
		348	vfree(saved->vga_font0);
		349	if (saved->vga_font1)
		350	vfree(saved->vga_font1);
		351	if (saved->vga_text)
		352	vfree(saved->vga_text);
		353	if (saved->vga_cmap)
		354	vfree(saved->vga_cmap);
		355	if (saved->attr)
		356	vfree(saved->attr);
		357	kfree(saved);
		358	state->vidstate = NULL;
		359	}
		360	}
		361
		362	int save_vga(struct vgastate *state)
		363	{
		364	struct regstate *saved;
		365
		366	saved = kmalloc(sizeof(struct regstate), GFP_KERNEL);
		367	if (saved == NULL)
		368	return 1;
		369	memset (saved, 0, sizeof(struct regstate));
1056	tullio	370
		371	// changed for gcc4 compatibility
		372	//(struct regstate *) state->vidstate = saved;
		373	state->vidstate = (struct regstate *) (saved);
468	giacomo	374
		375	if (state->flags & VGA_SAVE_CMAP) {
		376	saved->vga_cmap = vmalloc(768);
		377	if (!saved->vga_cmap) {
		378	vga_cleanup(state);
		379	return 1;
		380	}
		381	save_vga_cmap(state);
		382	}
		383
		384	if (state->flags & VGA_SAVE_MODE) {
		385	int total;
		386
		387	if (state->num_attr < 21)
		388	state->num_attr = 21;
		389	if (state->num_crtc < 25)
		390	state->num_crtc = 25;
		391	if (state->num_gfx < 9)
		392	state->num_gfx = 9;
		393	if (state->num_seq < 5)
		394	state->num_seq = 5;
		395	total = state->num_attr + state->num_crtc +
		396	state->num_gfx + state->num_seq;
		397
		398	saved->attr = vmalloc(total);
		399	if (!saved->attr) {
		400	vga_cleanup(state);
		401	return 1;
		402	}
		403	saved->crtc = saved->attr + state->num_attr;
		404	saved->gfx = saved->crtc + state->num_crtc;
		405	saved->seq = saved->gfx + state->num_gfx;
		406
		407	save_vga_mode(state);
		408	}
		409
		410	if (state->flags & VGA_SAVE_FONTS) {
		411	caddr_t fbbase;
		412
		413	/* exit if window is less than 32K */
		414	if (state->memsize && state->memsize < 4 * 8192) {
		415	vga_cleanup(state);
		416	return 1;
		417	}
		418	if (!state->memsize)
		419	state->memsize = 8 * 8192;
		420
		421	if (!state->membase)
		422	state->membase = 0xA0000;
		423
		424	fbbase = ioremap(state->membase, state->memsize);
		425
		426	if (!fbbase) {
		427	vga_cleanup(state);
		428	iounmap(fbbase);
		429	return 1;
		430	}
		431
		432	/*
		433	* save only first 32K used by vgacon
		434	*/
		435	if (state->flags & VGA_SAVE_FONT0) {
		436	saved->vga_font0 = vmalloc(4 * 8192);
		437	if (!saved->vga_font0) {
		438	vga_cleanup(state);
		439	return 1;
		440	}
		441	}
		442	/*
		443	* largely unused, but if required by the caller
		444	* we'll just save everything.
		445	*/
		446	if (state->flags & VGA_SAVE_FONT1) {
		447	saved->vga_font1 = vmalloc(state->memsize);
		448	if (!saved->vga_font1) {
		449	vga_cleanup(state);
		450	return 1;
		451	}
		452	}
		453	/*
		454	* Save 8K at plane0[0], and 8K at plane1[16K]
		455	*/
		456	if (state->flags & VGA_SAVE_TEXT) {
		457	saved->vga_text = vmalloc(8192 * 2);
		458	if (!saved->vga_text) {
		459	vga_cleanup(state);
		460	return 1;
		461	}
		462	}
		463
		464	save_vga_text(state, fbbase);
		465	iounmap(fbbase);
		466	}
		467	return 0;
		468	}
		469
		470	int restore_vga (struct vgastate *state)
		471	{
		472	if (state->vidstate == NULL)
		473	return 1;
		474
		475	if (state->flags & VGA_SAVE_MODE)
		476	restore_vga_mode(state);
		477
		478	if (state->flags & VGA_SAVE_FONTS) {
		479	caddr_t fbbase = ioremap(state->membase, state->memsize);
		480
		481	if (!fbbase) {
		482	vga_cleanup(state);
		483	iounmap(fbbase);
		484	return 1;
		485	}
		486	restore_vga_text(state, fbbase);
		487	iounmap(fbbase);
		488	}
		489
		490	if (state->flags & VGA_SAVE_CMAP)
		491	restore_vga_cmap(state);
		492
		493	vga_cleanup(state);
		494	return 0;
		495	}
		496
		497	#ifdef MODULE
		498	int init_module(void) { return 0; };
		499	void cleanup_module(void) {};
		500	#endif
		501
		502	EXPORT_SYMBOL(save_vga);
		503	EXPORT_SYMBOL(restore_vga);
		504
		505	MODULE_AUTHOR("James Simmons <jsimmons@users.sf.net>");
		506	MODULE_DESCRIPTION("VGA State Save/Restore");
		507	MODULE_LICENSE("GPL");
		508

Subversion Repositories shark

(root)/shark/trunk/drivers/fb/vgastate.c - Rev 1056