vga: vgastate fix
[linux-2.6.git] / drivers / video / vgastate.c
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
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
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
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
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                               
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
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
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
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
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                 
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
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