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