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 
22 struct 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 
vga_rcrtcs(void __iomem * regbase,unsigned short iobase,unsigned char reg)34 static 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 
vga_wcrtcs(void __iomem * regbase,unsigned short iobase,unsigned char reg,unsigned char val)41 static 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 
save_vga_text(struct vgastate * state,void __iomem * fbbase)48 static 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 
restore_vga_text(struct vgastate * state,void __iomem * fbbase)138 static 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 
save_vga_mode(struct vgastate * state)226 static 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 
restore_vga_mode(struct vgastate * state)257 static 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 
save_vga_cmap(struct vgastate * state)309 static 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 
restore_vga_cmap(struct vgastate * state)322 static 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 
vga_cleanup(struct vgastate * state)335 static 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 
save_vga(struct vgastate * state)350 int 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 
restore_vga(struct vgastate * state)458 int 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 
484 EXPORT_SYMBOL(save_vga);
485 EXPORT_SYMBOL(restore_vga);
486 
487 MODULE_AUTHOR("James Simmons <jsimmons@users.sf.net>");
488 MODULE_DESCRIPTION("VGA State Save/Restore");
489 MODULE_LICENSE("GPL");
490 
491