[linux.git] / drivers / video / vgastate.c

/*
 * 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 <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(void __iomem *regbase, unsigned short iobase, 
				       unsigned char reg)
{
	vga_w(regbase, iobase + 0x4, reg);
	return vga_r(regbase, iobase + 0x5);
}

static inline void vga_wcrtcs(void __iomem *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, void __iomem *fbbase)
{
	struct regstate *saved = (struct regstate *) state->vidstate;
	int i;
	u8 misc, attr10, gr4, gr5, gr6, seq1, seq2, seq4;
	unsigned short iobase;

	/* if in graphics mode, no need to save */
	misc = vga_r(state->vgabase, VGA_MIS_R);
	iobase = (misc & 1) ? 0x3d0 : 0x3b0;

	vga_r(state->vgabase, iobase + 0xa);
	vga_w(state->vgabase, VGA_ATT_W, 0x00);
	attr10 = vga_rattr(state->vgabase, 0x10);
	vga_r(state->vgabase, iobase + 0xa);
	vga_w(state->vgabase, VGA_ATT_W, 0x20);

	if (attr10 & 1)
		return;

	/* save regs */
	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);
	
	/* 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_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);

	/* 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, void __iomem *fbbase)
{
	struct regstate *saved = (struct regstate *) state->vidstate;
	int i;
	u8 gr1, gr3, gr4, gr5, gr6, gr8;
	u8 seq1, seq2, seq4;

	/* save regs */
	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);
	
	/* 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_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;

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

	saved = kzalloc(sizeof(struct regstate), GFP_KERNEL);

	if (saved == NULL)
		return 1;

	state->vidstate = (void *)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) {
		void __iomem *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);
			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) {
				iounmap(fbbase);
				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) {
				iounmap(fbbase);
				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) {
				iounmap(fbbase);
				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) {
		void __iomem *fbbase = ioremap(state->membase, state->memsize);

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

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

	vga_cleanup(state);
	return 0;
}

EXPORT_SYMBOL(save_vga);
EXPORT_SYMBOL(restore_vga);

MODULE_AUTHOR("James Simmons <[email protected]>");
MODULE_DESCRIPTION("VGA State Save/Restore");
MODULE_LICENSE("GPL");
Commit	Line	Data
1da177e4 LT	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	*/
1da177e4 LT	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;
0449359f	53	unsigned short iobase;
1da177e4 LT	54
1da177e4 LT	55	/* if in graphics mode, no need to save */
0449359f OZ	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);
1da177e4	61	attr10 = vga_rattr(state->vgabase, 0x10);
0449359f OZ	62	vga_r(state->vgabase, iobase + 0xa);
	63	vga_w(state->vgabase, VGA_ATT_W, 0x20);
	64
1da177e4 LT	65	if (attr10 & 1)
1da177e4 LT	66	return;
0449359f	67
1da177e4	68	/* save regs */
1da177e4 LT	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
1da177e4 LT	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 */
1da177e4 LT	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);
1da177e4 LT	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;
0449359f	142	u8 gr1, gr3, gr4, gr5, gr6, gr8;
1da177e4 LT	143	u8 seq1, seq2, seq4;
	144
	145	/* save regs */
1da177e4 LT	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
1da177e4 LT	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 */
1da177e4 LT	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
f9101210 JJ	340	vfree(saved->vga_font0);
	341	vfree(saved->vga_font1);
	342	vfree(saved->vga_text);
	343	vfree(saved->vga_cmap);
	344	vfree(saved->attr);
1da177e4 LT	345	kfree(saved);
	346	state->vidstate = NULL;
	347	}
	348	}
	349
	350	int save_vga(struct vgastate *state)
	351	{
	352	struct regstate *saved;
	353
def1eded AD	354	saved = kzalloc(sizeof(struct regstate), GFP_KERNEL);
def1eded AD	355
1da177e4 LT	356	if (saved == NULL)
1da177e4 LT	357	return 1;
def1eded	358
1da177e4 LT	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
1da177e4 LT	484	EXPORT_SYMBOL(save_vga);
	485	EXPORT_SYMBOL(restore_vga);
	486
	487	MODULE_AUTHOR("James Simmons <[email protected]>");
	488	MODULE_DESCRIPTION("VGA State Save/Restore");
	489	MODULE_LICENSE("GPL");
	490