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