Subversion Repositories shark

Rev

Rev 468 | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*
 * linux/drivers/video/vgastate.c -- VGA state save/restore
 *
 * Copyright 2002 James Simmons
 *
 * Copyright history from vga16fb.c:
 *      Copyright 1999 Ben Pfaff and Petr Vandrovec
 *      Based on VGA info at http://www.goodnet.com/~tinara/FreeVGA/home.htm
 *      Based on VESA framebuffer (c) 1998 Gerd Knorr
 *
 * This file is subject to the terms and conditions of the GNU General
 * Public License.  See the file COPYING in the main directory of this
 * archive for more details.  
 *
 */
#include <linuxcomp.h>

#include <linux/config.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/fb.h>
#include <linux/vmalloc.h>
#include <video/vga.h>

struct regstate {
        __u8 *vga_font0;
        __u8 *vga_font1;
        __u8 *vga_text;
        __u8 *vga_cmap;
        __u8 *attr;
        __u8 *crtc;
        __u8 *gfx;
        __u8 *seq;
        __u8 misc;
};     

static inline unsigned char vga_rcrtcs(caddr_t regbase, unsigned short iobase,
                                       unsigned char reg)
{
        vga_w(regbase, iobase + 0x4, reg);
        return vga_r(regbase, iobase + 0x5);
}

static inline void vga_wcrtcs(caddr_t regbase, unsigned short iobase,
                              unsigned char reg, unsigned char val)
{
        vga_w(regbase, iobase + 0x4, reg);
        vga_w(regbase, iobase + 0x5, val);
}

static void save_vga_text(struct vgastate *state, caddr_t fbbase)
{
        struct regstate *saved = (struct regstate *) state->vidstate;
        int i;
        u8 misc, attr10, gr4, gr5, gr6, seq1, seq2, seq4;

        /* if in graphics mode, no need to save */
        attr10 = vga_rattr(state->vgabase, 0x10);
        if (attr10 & 1)
                return;
       
        /* save regs */
        misc = vga_r(state->vgabase, VGA_MIS_R);
        gr4 = vga_rgfx(state->vgabase, VGA_GFX_PLANE_READ);
        gr5 = vga_rgfx(state->vgabase, VGA_GFX_MODE);
        gr6 = vga_rgfx(state->vgabase, VGA_GFX_MISC);
        seq2 = vga_rseq(state->vgabase, VGA_SEQ_PLANE_WRITE);
        seq4 = vga_rseq(state->vgabase, VGA_SEQ_MEMORY_MODE);
       
        /* force graphics mode */
        vga_w(state->vgabase, VGA_MIS_W, misc | 1);

        /* blank screen */
        seq1 = vga_rseq(state->vgabase, VGA_SEQ_CLOCK_MODE);
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
        vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1 | 1 << 5);
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x3);

        /* save font at plane 2 */
        if (state->flags & VGA_SAVE_FONT0) {
                vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x4);
                vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
                vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x2);
                vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
                for (i = 0; i < 4 * 8192; i++)
                        saved->vga_font0[i] = vga_r(fbbase, i);
        }

        /* save font at plane 3 */
        if (state->flags & VGA_SAVE_FONT1) {
                vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x8);
                vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
                vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x3);
                vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
                for (i = 0; i < state->memsize; i++)
                        saved->vga_font1[i] = vga_r(fbbase, i);
        }
       
        /* save font at plane 0/1 */
        if (state->flags & VGA_SAVE_TEXT) {
                vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x1);
                vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
                vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
                for (i = 0; i < 8192; i++)
                        saved->vga_text[i] = vga_r(fbbase, i);

                vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x2);
                vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
                vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x1);
                vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
                for (i = 0; i < 8192; i++)
                        saved->vga_text[8192+i] = vga_r(fbbase + 2 * 8192, i);
        }

        /* restore regs */
        vga_wattr(state->vgabase, 0x10, attr10);

        vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, seq2);
        vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, seq4);

        vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, gr4);
        vga_wgfx(state->vgabase, VGA_GFX_MODE, gr5);
        vga_wgfx(state->vgabase, VGA_GFX_MISC, gr6);
        vga_w(state->vgabase, VGA_MIS_W, misc);

        /* unblank screen */
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
        vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1 & ~(1 << 5));
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x3);

        vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1);
}

static void restore_vga_text(struct vgastate *state, caddr_t fbbase)
{
        struct regstate *saved = (struct regstate *) state->vidstate;
        int i;
        u8 misc, gr1, gr3, gr4, gr5, gr6, gr8;
        u8 seq1, seq2, seq4;

        /* save regs */
        misc = vga_r(state->vgabase, VGA_MIS_R);
        gr1 = vga_rgfx(state->vgabase, VGA_GFX_SR_ENABLE);
        gr3 = vga_rgfx(state->vgabase, VGA_GFX_DATA_ROTATE);
        gr4 = vga_rgfx(state->vgabase, VGA_GFX_PLANE_READ);
        gr5 = vga_rgfx(state->vgabase, VGA_GFX_MODE);
        gr6 = vga_rgfx(state->vgabase, VGA_GFX_MISC);
        gr8 = vga_rgfx(state->vgabase, VGA_GFX_BIT_MASK);
        seq2 = vga_rseq(state->vgabase, VGA_SEQ_PLANE_WRITE);
        seq4 = vga_rseq(state->vgabase, VGA_SEQ_MEMORY_MODE);
       
        /* force graphics mode */
        vga_w(state->vgabase, VGA_MIS_W, misc | 1);

        /* blank screen */
        seq1 = vga_rseq(state->vgabase, VGA_SEQ_CLOCK_MODE);
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
        vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1 | 1 << 5);
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x3);

        if (state->depth == 4) {
                vga_wgfx(state->vgabase, VGA_GFX_DATA_ROTATE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_BIT_MASK, 0xff);
                vga_wgfx(state->vgabase, VGA_GFX_SR_ENABLE, 0x00);
        }
       
        /* restore font at plane 2 */
        if (state->flags & VGA_SAVE_FONT0) {
                vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x4);
                vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
                vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x2);
                vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
                for (i = 0; i < 4 * 8192; i++)
                        vga_w(fbbase, i, saved->vga_font0[i]);
        }

        /* restore font at plane 3 */
        if (state->flags & VGA_SAVE_FONT1) {
                vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x8);
                vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
                vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x3);
                vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
                for (i = 0; i < state->memsize; i++)
                        vga_w(fbbase, i, saved->vga_font1[i]);
        }
       
        /* restore font at plane 0/1 */
        if (state->flags & VGA_SAVE_TEXT) {
                vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x1);
                vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
                vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
                for (i = 0; i < 8192; i++)
                        vga_w(fbbase, i, saved->vga_text[i]);
               
                vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, 0x2);
                vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, 0x6);
                vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, 0x1);
                vga_wgfx(state->vgabase, VGA_GFX_MODE, 0x0);
                vga_wgfx(state->vgabase, VGA_GFX_MISC, 0x5);
                for (i = 0; i < 8192; i++)
                        vga_w(fbbase, i, saved->vga_text[8192+i]);
        }

        /* unblank screen */
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x1);
        vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1 & ~(1 << 5));
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x3);

        /* restore regs */
        vga_w(state->vgabase, VGA_MIS_W, misc);

        vga_wgfx(state->vgabase, VGA_GFX_SR_ENABLE, gr1);
        vga_wgfx(state->vgabase, VGA_GFX_DATA_ROTATE, gr3);
        vga_wgfx(state->vgabase, VGA_GFX_PLANE_READ, gr4);
        vga_wgfx(state->vgabase, VGA_GFX_MODE, gr5);
        vga_wgfx(state->vgabase, VGA_GFX_MISC, gr6);
        vga_wgfx(state->vgabase, VGA_GFX_BIT_MASK, gr8);

        vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE, seq1);
        vga_wseq(state->vgabase, VGA_SEQ_PLANE_WRITE, seq2);
        vga_wseq(state->vgabase, VGA_SEQ_MEMORY_MODE, seq4);
}
                             
static void save_vga_mode(struct vgastate *state)
{
        struct regstate *saved = (struct regstate *) state->vidstate;
        unsigned short iobase;
        int i;

        saved->misc = vga_r(state->vgabase, VGA_MIS_R);
        if (saved->misc & 1)
                iobase = 0x3d0;
        else
                iobase = 0x3b0;

        for (i = 0; i < state->num_crtc; i++)
                saved->crtc[i] = vga_rcrtcs(state->vgabase, iobase, i);
       
        vga_r(state->vgabase, iobase + 0xa);
        vga_w(state->vgabase, VGA_ATT_W, 0x00);
        for (i = 0; i < state->num_attr; i++) {
                vga_r(state->vgabase, iobase + 0xa);
                saved->attr[i] = vga_rattr(state->vgabase, i);
        }
        vga_r(state->vgabase, iobase + 0xa);
        vga_w(state->vgabase, VGA_ATT_W, 0x20);

        for (i = 0; i < state->num_gfx; i++)
                saved->gfx[i] = vga_rgfx(state->vgabase, i);

        for (i = 0; i < state->num_seq; i++)
                saved->seq[i] = vga_rseq(state->vgabase, i);
}

static void restore_vga_mode(struct vgastate *state)
{
        struct regstate *saved = (struct regstate *) state->vidstate;
        unsigned short iobase;
        int i;

        vga_w(state->vgabase, VGA_MIS_W, saved->misc);

        if (saved->misc & 1)
                iobase = 0x3d0;
        else
                iobase = 0x3b0;

        /* turn off display */
        vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE,
                 saved->seq[VGA_SEQ_CLOCK_MODE] | 0x20);

        /* disable sequencer */
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x01);
       
        /* enable palette addressing */
        vga_r(state->vgabase, iobase + 0xa);
        vga_w(state->vgabase, VGA_ATT_W, 0x00);

        for (i = 2; i < state->num_seq; i++)
                vga_wseq(state->vgabase, i, saved->seq[i]);


        /* unprotect vga regs */
        vga_wcrtcs(state->vgabase, iobase, 17, saved->crtc[17] & ~0x80);
        for (i = 0; i < state->num_crtc; i++)
                vga_wcrtcs(state->vgabase, iobase, i, saved->crtc[i]);
       
        for (i = 0; i < state->num_gfx; i++)
                vga_wgfx(state->vgabase, i, saved->gfx[i]);

        for (i = 0; i < state->num_attr; i++) {
                vga_r(state->vgabase, iobase + 0xa);
                vga_wattr(state->vgabase, i, saved->attr[i]);
        }

        /* reenable sequencer */
        vga_wseq(state->vgabase, VGA_SEQ_RESET, 0x03);
        /* turn display on */
        vga_wseq(state->vgabase, VGA_SEQ_CLOCK_MODE,
                 saved->seq[VGA_SEQ_CLOCK_MODE] & ~(1 << 5));

        /* disable video/palette source */
        vga_r(state->vgabase, iobase + 0xa);
        vga_w(state->vgabase, VGA_ATT_W, 0x20);
}

static void save_vga_cmap(struct vgastate *state)
{
        struct regstate *saved = (struct regstate *) state->vidstate;
        int i;

        vga_w(state->vgabase, VGA_PEL_MSK, 0xff);
       
        /* assumes DAC is readable and writable */
        vga_w(state->vgabase, VGA_PEL_IR, 0x00);
        for (i = 0; i < 768; i++)
                saved->vga_cmap[i] = vga_r(state->vgabase, VGA_PEL_D);
}

static void restore_vga_cmap(struct vgastate *state)
{
        struct regstate *saved = (struct regstate *) state->vidstate;
        int i;

        vga_w(state->vgabase, VGA_PEL_MSK, 0xff);

        /* assumes DAC is readable and writable */
        vga_w(state->vgabase, VGA_PEL_IW, 0x00);
        for (i = 0; i < 768; i++)
                vga_w(state->vgabase, VGA_PEL_D, saved->vga_cmap[i]);
}

static void vga_cleanup(struct vgastate *state)
{
        if (state->vidstate != NULL) {
                struct regstate *saved = (struct regstate *) state->vidstate;

                if (saved->vga_font0)
                        vfree(saved->vga_font0);
                if (saved->vga_font1)
                        vfree(saved->vga_font1);
                if (saved->vga_text)
                        vfree(saved->vga_text);
                if (saved->vga_cmap)
                        vfree(saved->vga_cmap);
                if (saved->attr)
                        vfree(saved->attr);
                kfree(saved);
                state->vidstate = NULL;
        }
}
               
int save_vga(struct vgastate *state)
{
        struct regstate *saved;

        saved = kmalloc(sizeof(struct regstate), GFP_KERNEL);
        if (saved == NULL)
                return 1;
        memset (saved, 0, sizeof(struct regstate));
       
        // changed for gcc4 compatibility
        //(struct regstate *) state->vidstate = saved;
        state->vidstate = (struct regstate *) (saved);
               
        if (state->flags & VGA_SAVE_CMAP) {
                saved->vga_cmap = vmalloc(768);
                if (!saved->vga_cmap) {
                        vga_cleanup(state);
                        return 1;
                }
                save_vga_cmap(state);
        }

        if (state->flags & VGA_SAVE_MODE) {
                int total;

                if (state->num_attr < 21)
                        state->num_attr = 21;
                if (state->num_crtc < 25)
                        state->num_crtc = 25;
                if (state->num_gfx < 9)
                        state->num_gfx = 9;
                if (state->num_seq < 5)
                        state->num_seq = 5;
                total = state->num_attr + state->num_crtc +
                        state->num_gfx + state->num_seq;

                saved->attr = vmalloc(total);
                if (!saved->attr) {
                        vga_cleanup(state);
                        return 1;
                }
                saved->crtc = saved->attr + state->num_attr;
                saved->gfx = saved->crtc + state->num_crtc;
                saved->seq = saved->gfx + state->num_gfx;

                save_vga_mode(state);
        }

        if (state->flags & VGA_SAVE_FONTS) {
                caddr_t fbbase;

                /* exit if window is less than 32K */
                if (state->memsize && state->memsize < 4 * 8192) {
                        vga_cleanup(state);
                        return 1;
                }
                if (!state->memsize)
                        state->memsize = 8 * 8192;
               
                if (!state->membase)
                        state->membase = 0xA0000;

                fbbase = ioremap(state->membase, state->memsize);

                if (!fbbase) {
                        vga_cleanup(state);
                        iounmap(fbbase);
                        return 1;
        }

                /*
                 * save only first 32K used by vgacon
                 */
                if (state->flags & VGA_SAVE_FONT0) {
                        saved->vga_font0 = vmalloc(4 * 8192);
                        if (!saved->vga_font0) {
                        vga_cleanup(state);
                        return 1;
                }
        }
                /*
                 * largely unused, but if required by the caller
                 * we'll just save everything.
                 */
                if (state->flags & VGA_SAVE_FONT1) {
                        saved->vga_font1 = vmalloc(state->memsize);
                        if (!saved->vga_font1) {
                        vga_cleanup(state);
                        return 1;
                }
        }
                /*
                 * Save 8K at plane0[0], and 8K at plane1[16K]
                 */
                if (state->flags & VGA_SAVE_TEXT) {
                        saved->vga_text = vmalloc(8192 * 2);
                        if (!saved->vga_text) {
                        vga_cleanup(state);
                        return 1;
                }
                }
               
                save_vga_text(state, fbbase);
                iounmap(fbbase);
        }
        return 0;
}

int restore_vga (struct vgastate *state)
{
        if (state->vidstate == NULL)
                return 1;

        if (state->flags & VGA_SAVE_MODE)
                restore_vga_mode(state);

        if (state->flags & VGA_SAVE_FONTS) {
                caddr_t fbbase = ioremap(state->membase, state->memsize);

                if (!fbbase) {
                        vga_cleanup(state);
                        iounmap(fbbase);
                        return 1;
                }
                restore_vga_text(state, fbbase);
                iounmap(fbbase);
        }

        if (state->flags & VGA_SAVE_CMAP)
                restore_vga_cmap(state);

        vga_cleanup(state);
        return 0;
}

#ifdef MODULE
int init_module(void) { return 0; };
void cleanup_module(void) {};
#endif

EXPORT_SYMBOL(save_vga);
EXPORT_SYMBOL(restore_vga);

MODULE_AUTHOR("James Simmons <jsimmons@users.sf.net>");
MODULE_DESCRIPTION("VGA State Save/Restore");
MODULE_LICENSE("GPL");