7 files changed, 165 insertions, 118 deletions

diff --git a/drivers/video/nvidia/nv_accel.c b/drivers/video/nvidia/nv_accel.c
index 9efb8a3854e2..fa4821c5572b 100644
--- a/drivers/video/nvidia/nv_accel.c
+++ b/drivers/video/nvidia/nv_accel.c
@@ -69,27 +69,38 @@ static const int NVCopyROP_PM[16] = {
 	0x5A,			/* invert */
 };
 
-static inline void NVFlush(struct nvidia_par *par)
+static inline void nvidiafb_safe_mode(struct fb_info *info)
 {
+	struct nvidia_par *par = info->par;
+
+	touch_softlockup_watchdog();
+	info->pixmap.scan_align = 1;
+	par->lockup = 1;
+}
+
+static inline void NVFlush(struct fb_info *info)
+{
+	struct nvidia_par *par = info->par;
 	int count = 1000000000;
 
 	while (--count && READ_GET(par) != par->dmaPut) ;
 
 	if (!count) {
 		printk("nvidiafb: DMA Flush lockup\n");
-		par->lockup = 1;
+		nvidiafb_safe_mode(info);
 	}
 }
 
-static inline void NVSync(struct nvidia_par *par)
+static inline void NVSync(struct fb_info *info)
 {
+	struct nvidia_par *par = info->par;
 	int count = 1000000000;
 
 	while (--count && NV_RD32(par->PGRAPH, 0x0700)) ;
 
 	if (!count) {
 		printk("nvidiafb: DMA Sync lockup\n");
-		par->lockup = 1;
+		nvidiafb_safe_mode(info);
 	}
 }
 
@@ -101,8 +112,9 @@ static void NVDmaKickoff(struct nvidia_par *par)
 	}
 }
 
-static void NVDmaWait(struct nvidia_par *par, int size)
+static void NVDmaWait(struct fb_info *info, int size)
 {
+	struct nvidia_par *par = info->par;
 	int dmaGet;
 	int count = 1000000000, cnt;
 	size++;
@@ -135,34 +147,38 @@ static void NVDmaWait(struct nvidia_par *par, int size)
 	}
 
 	if (!count) {
-		printk("DMA Wait Lockup\n");
-		par->lockup = 1;
+		printk("nvidiafb: DMA Wait Lockup\n");
+		nvidiafb_safe_mode(info);
 	}
 }
 
-static void NVSetPattern(struct nvidia_par *par, u32 clr0, u32 clr1,
+static void NVSetPattern(struct fb_info *info, u32 clr0, u32 clr1,
 			 u32 pat0, u32 pat1)
 {
-	NVDmaStart(par, PATTERN_COLOR_0, 4);
+	struct nvidia_par *par = info->par;
+
+	NVDmaStart(info, par, PATTERN_COLOR_0, 4);
 	NVDmaNext(par, clr0);
 	NVDmaNext(par, clr1);
 	NVDmaNext(par, pat0);
 	NVDmaNext(par, pat1);
 }
 
-static void NVSetRopSolid(struct nvidia_par *par, u32 rop, u32 planemask)
+static void NVSetRopSolid(struct fb_info *info, u32 rop, u32 planemask)
 {
+	struct nvidia_par *par = info->par;
+
 	if (planemask != ~0) {
-		NVSetPattern(par, 0, planemask, ~0, ~0);
+		NVSetPattern(info, 0, planemask, ~0, ~0);
 		if (par->currentRop != (rop + 32)) {
-			NVDmaStart(par, ROP_SET, 1);
+			NVDmaStart(info, par, ROP_SET, 1);
 			NVDmaNext(par, NVCopyROP_PM[rop]);
 			par->currentRop = rop + 32;
 		}
 	} else if (par->currentRop != rop) {
 		if (par->currentRop >= 16)
-			NVSetPattern(par, ~0, ~0, ~0, ~0);
-		NVDmaStart(par, ROP_SET, 1);
+			NVSetPattern(info, ~0, ~0, ~0, ~0);
+		NVDmaStart(info, par, ROP_SET, 1);
 		NVDmaNext(par, NVCopyROP[rop]);
 		par->currentRop = rop;
 	}
@@ -175,7 +191,7 @@ static void NVSetClippingRectangle(struct fb_info *info, int x1, int y1,
 	int h = y2 - y1 + 1;
 	int w = x2 - x1 + 1;
 
-	NVDmaStart(par, CLIP_POINT, 2);
+	NVDmaStart(info, par, CLIP_POINT, 2);
 	NVDmaNext(par, (y1 << 16) | x1);
 	NVDmaNext(par, (h << 16) | w);
 }
@@ -237,23 +253,23 @@ void NVResetGraphics(struct fb_info *info)
 		break;
 	}
 
-	NVDmaStart(par, SURFACE_FORMAT, 4);
+	NVDmaStart(info, par, SURFACE_FORMAT, 4);
 	NVDmaNext(par, surfaceFormat);
 	NVDmaNext(par, pitch | (pitch << 16));
 	NVDmaNext(par, 0);
 	NVDmaNext(par, 0);
 
-	NVDmaStart(par, PATTERN_FORMAT, 1);
+	NVDmaStart(info, par, PATTERN_FORMAT, 1);
 	NVDmaNext(par, patternFormat);
 
-	NVDmaStart(par, RECT_FORMAT, 1);
+	NVDmaStart(info, par, RECT_FORMAT, 1);
 	NVDmaNext(par, rectFormat);
 
-	NVDmaStart(par, LINE_FORMAT, 1);
+	NVDmaStart(info, par, LINE_FORMAT, 1);
 	NVDmaNext(par, lineFormat);
 
 	par->currentRop = ~0;	/* set to something invalid */
-	NVSetRopSolid(par, ROP_COPY, ~0);
+	NVSetRopSolid(info, ROP_COPY, ~0);
 
 	NVSetClippingRectangle(info, 0, 0, info->var.xres_virtual,
 			       info->var.yres_virtual);
@@ -269,10 +285,10 @@ int nvidiafb_sync(struct fb_info *info)
 		return 0;
 
 	if (!par->lockup)
-		NVFlush(par);
+		NVFlush(info);
 
 	if (!par->lockup)
-		NVSync(par);
+		NVSync(info);
 
 	return 0;
 }
@@ -287,7 +303,7 @@ void nvidiafb_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (par->lockup)
 		return cfb_copyarea(info, region);
 
-	NVDmaStart(par, BLIT_POINT_SRC, 3);
+	NVDmaStart(info, par, BLIT_POINT_SRC, 3);
 	NVDmaNext(par, (region->sy << 16) | region->sx);
 	NVDmaNext(par, (region->dy << 16) | region->dx);
 	NVDmaNext(par, (region->height << 16) | region->width);
@@ -312,19 +328,19 @@ void nvidiafb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 		color = ((u32 *) info->pseudo_palette)[rect->color];
 
 	if (rect->rop != ROP_COPY)
-		NVSetRopSolid(par, rect->rop, ~0);
+		NVSetRopSolid(info, rect->rop, ~0);
 
-	NVDmaStart(par, RECT_SOLID_COLOR, 1);
+	NVDmaStart(info, par, RECT_SOLID_COLOR, 1);
 	NVDmaNext(par, color);
 
-	NVDmaStart(par, RECT_SOLID_RECTS(0), 2);
+	NVDmaStart(info, par, RECT_SOLID_RECTS(0), 2);
 	NVDmaNext(par, (rect->dx << 16) | rect->dy);
 	NVDmaNext(par, (rect->width << 16) | rect->height);
 
 	NVDmaKickoff(par);
 
 	if (rect->rop != ROP_COPY)
-		NVSetRopSolid(par, ROP_COPY, ~0);
+		NVSetRopSolid(info, ROP_COPY, ~0);
 }
 
 static void nvidiafb_mono_color_expand(struct fb_info *info,
@@ -346,7 +362,7 @@ static void nvidiafb_mono_color_expand(struct fb_info *info,
 		bg = ((u32 *) info->pseudo_palette)[image->bg_color] | mask;
 	}
 
-	NVDmaStart(par, RECT_EXPAND_TWO_COLOR_CLIP, 7);
+	NVDmaStart(info, par, RECT_EXPAND_TWO_COLOR_CLIP, 7);
 	NVDmaNext(par, (image->dy << 16) | (image->dx & 0xffff));
 	NVDmaNext(par, ((image->dy + image->height) << 16) |
 		  ((image->dx + image->width) & 0xffff));
@@ -357,7 +373,7 @@ static void nvidiafb_mono_color_expand(struct fb_info *info,
 	NVDmaNext(par, (image->dy << 16) | (image->dx & 0xffff));
 
 	while (dsize >= RECT_EXPAND_TWO_COLOR_DATA_MAX_DWORDS) {
-		NVDmaStart(par, RECT_EXPAND_TWO_COLOR_DATA(0),
+		NVDmaStart(info, par, RECT_EXPAND_TWO_COLOR_DATA(0),
 			   RECT_EXPAND_TWO_COLOR_DATA_MAX_DWORDS);
 
 		for (j = RECT_EXPAND_TWO_COLOR_DATA_MAX_DWORDS; j--;) {
@@ -370,7 +386,7 @@ static void nvidiafb_mono_color_expand(struct fb_info *info,
 	}
 
 	if (dsize) {
-		NVDmaStart(par, RECT_EXPAND_TWO_COLOR_DATA(0), dsize);
+		NVDmaStart(info, par, RECT_EXPAND_TWO_COLOR_DATA(0), dsize);
 
 		for (j = dsize; j--;) {
 			tmp = data[k++];
diff --git a/drivers/video/nvidia/nv_hw.c b/drivers/video/nvidia/nv_hw.c
index ea426115c6f9..f297c7b14a41 100644
--- a/drivers/video/nvidia/nv_hw.c
+++ b/drivers/video/nvidia/nv_hw.c
@@ -686,7 +686,7 @@ static void nForceUpdateArbitrationSettings(unsigned VClk,
 
 	if ((par->Chipset & 0x0FF0) == 0x01A0) {
 		unsigned int uMClkPostDiv;
-		dev = pci_find_slot(0, 3);
+		dev = pci_get_bus_and_slot(0, 3);
 		pci_read_config_dword(dev, 0x6C, &uMClkPostDiv);
 		uMClkPostDiv = (uMClkPostDiv >> 8) & 0xf;
 
@@ -694,11 +694,11 @@ static void nForceUpdateArbitrationSettings(unsigned VClk,
 			uMClkPostDiv = 4;
 		MClk = 400000 / uMClkPostDiv;
 	} else {
-		dev = pci_find_slot(0, 5);
+		dev = pci_get_bus_and_slot(0, 5);
 		pci_read_config_dword(dev, 0x4c, &MClk);
 		MClk /= 1000;
 	}
-
+	pci_dev_put(dev);
 	pll = NV_RD32(par->PRAMDAC0, 0x0500);
 	M = (pll >> 0) & 0xFF;
 	N = (pll >> 8) & 0xFF;
@@ -707,19 +707,21 @@ static void nForceUpdateArbitrationSettings(unsigned VClk,
 	sim_data.pix_bpp = (char)pixelDepth;
 	sim_data.enable_video = 0;
 	sim_data.enable_mp = 0;
-	pci_find_slot(0, 1);
+	dev = pci_get_bus_and_slot(0, 1);
 	pci_read_config_dword(dev, 0x7C, &sim_data.memory_type);
+	pci_dev_put(dev);
 	sim_data.memory_type = (sim_data.memory_type >> 12) & 1;
 	sim_data.memory_width = 64;
 
-	dev = pci_find_slot(0, 3);
+	dev = pci_get_bus_and_slot(0, 3);
 	pci_read_config_dword(dev, 0, &memctrl);
+	pci_dev_put(dev);
 	memctrl >>= 16;
 
 	if ((memctrl == 0x1A9) || (memctrl == 0x1AB) || (memctrl == 0x1ED)) {
 		int dimm[3];
 
-		pci_find_slot(0, 2);
+		dev = pci_get_bus_and_slot(0, 2);
 		pci_read_config_dword(dev, 0x40, &dimm[0]);
 		dimm[0] = (dimm[0] >> 8) & 0x4f;
 		pci_read_config_dword(dev, 0x44, &dimm[1]);
@@ -731,6 +733,7 @@ static void nForceUpdateArbitrationSettings(unsigned VClk,
 			printk("nvidiafb: your nForce DIMMs are not arranged "
 			       "in optimal banks!\n");
 		}
+		pci_dev_put(dev);
 	}
 
 	sim_data.mem_latency = 3;
diff --git a/drivers/video/nvidia/nv_i2c.c b/drivers/video/nvidia/nv_i2c.c
index b8588973e400..afe4567e1ff4 100644
--- a/drivers/video/nvidia/nv_i2c.c
+++ b/drivers/video/nvidia/nv_i2c.c
@@ -30,16 +30,14 @@ static void nvidia_gpio_setscl(void *data, int state)
 	struct nvidia_par *par = chan->par;
 	u32 val;
 
-	VGA_WR08(par->PCIO, 0x3d4, chan->ddc_base + 1);
-	val = VGA_RD08(par->PCIO, 0x3d5) & 0xf0;
+	val = NVReadCrtc(par, chan->ddc_base + 1) & 0xf0;
 
 	if (state)
 		val |= 0x20;
 	else
 		val &= ~0x20;
 
-	VGA_WR08(par->PCIO, 0x3d4, chan->ddc_base + 1);
-	VGA_WR08(par->PCIO, 0x3d5, val | 0x1);
+	NVWriteCrtc(par, chan->ddc_base + 1, val | 0x01);
 }
 
 static void nvidia_gpio_setsda(void *data, int state)
@@ -48,16 +46,14 @@ static void nvidia_gpio_setsda(void *data, int state)
 	struct nvidia_par *par = chan->par;
 	u32 val;
 
-	VGA_WR08(par->PCIO, 0x3d4, chan->ddc_base + 1);
-	val = VGA_RD08(par->PCIO, 0x3d5) & 0xf0;
+	val = NVReadCrtc(par, chan->ddc_base + 1) & 0xf0;
 
 	if (state)
 		val |= 0x10;
 	else
 		val &= ~0x10;
 
-	VGA_WR08(par->PCIO, 0x3d4, chan->ddc_base + 1);
-	VGA_WR08(par->PCIO, 0x3d5, val | 0x1);
+	NVWriteCrtc(par, chan->ddc_base + 1, val | 0x01);
 }
 
 static int nvidia_gpio_getscl(void *data)
@@ -66,12 +62,9 @@ static int nvidia_gpio_getscl(void *data)
 	struct nvidia_par *par = chan->par;
 	u32 val = 0;
 
-	VGA_WR08(par->PCIO, 0x3d4, chan->ddc_base);
-	if (VGA_RD08(par->PCIO, 0x3d5) & 0x04)
+	if (NVReadCrtc(par, chan->ddc_base) & 0x04)
 		val = 1;
 
-	val = VGA_RD08(par->PCIO, 0x3d5);
-
 	return val;
 }
 
@@ -81,20 +74,21 @@ static int nvidia_gpio_getsda(void *data)
 	struct nvidia_par *par = chan->par;
 	u32 val = 0;
 
-	VGA_WR08(par->PCIO, 0x3d4, chan->ddc_base);
-	if (VGA_RD08(par->PCIO, 0x3d5) & 0x08)
+	if (NVReadCrtc(par, chan->ddc_base) & 0x08)
 		val = 1;
 
 	return val;
 }
 
-static int nvidia_setup_i2c_bus(struct nvidia_i2c_chan *chan, const char *name)
+static int nvidia_setup_i2c_bus(struct nvidia_i2c_chan *chan, const char *name,
+				unsigned int i2c_class)
 {
 	int rc;
 
 	strcpy(chan->adapter.name, name);
 	chan->adapter.owner = THIS_MODULE;
 	chan->adapter.id = I2C_HW_B_NVIDIA;
+	chan->adapter.class = i2c_class;
 	chan->adapter.algo_data = &chan->algo;
 	chan->adapter.dev.parent = &chan->par->pci_dev->dev;
 	chan->algo.setsda = nvidia_gpio_setsda;
@@ -127,83 +121,39 @@ static int nvidia_setup_i2c_bus(struct nvidia_i2c_chan *chan, const char *name)
 
 void nvidia_create_i2c_busses(struct nvidia_par *par)
 {
-	par->bus = 3;
-
 	par->chan[0].par = par;
 	par->chan[1].par = par;
 	par->chan[2].par = par;
 
-	par->chan[0].ddc_base = 0x3e;
-	nvidia_setup_i2c_bus(&par->chan[0], "nvidia #0");
+	par->chan[0].ddc_base = 0x36;
+ 	nvidia_setup_i2c_bus(&par->chan[0], "nvidia #0", I2C_CLASS_HWMON);
 
-	par->chan[1].ddc_base = 0x36;
-	nvidia_setup_i2c_bus(&par->chan[1], "nvidia #1");
+	par->chan[1].ddc_base = 0x3e;
+ 	nvidia_setup_i2c_bus(&par->chan[1], "nvidia #1", 0);
 
 	par->chan[2].ddc_base = 0x50;
-	nvidia_setup_i2c_bus(&par->chan[2], "nvidia #2");
+ 	nvidia_setup_i2c_bus(&par->chan[2], "nvidia #2", 0);
 }
 
 void nvidia_delete_i2c_busses(struct nvidia_par *par)
 {
-	if (par->chan[0].par)
-		i2c_del_adapter(&par->chan[0].adapter);
-	par->chan[0].par = NULL;
-
-	if (par->chan[1].par)
-		i2c_del_adapter(&par->chan[1].adapter);
-	par->chan[1].par = NULL;
-
-	if (par->chan[2].par)
-		i2c_del_adapter(&par->chan[2].adapter);
-	par->chan[2].par = NULL;
-
-}
+	int i;
 
-static u8 *nvidia_do_probe_i2c_edid(struct nvidia_i2c_chan *chan)
-{
-	u8 start = 0x0;
-	struct i2c_msg msgs[] = {
-		{
-		 .addr = 0x50,
-		 .len = 1,
-		 .buf = &start,
-		 }, {
-		     .addr = 0x50,
-		     .flags = I2C_M_RD,
-		     .len = EDID_LENGTH,
-		     },
-	};
-	u8 *buf;
-
-	if (!chan->par)
-		return NULL;
-
-	buf = kmalloc(EDID_LENGTH, GFP_KERNEL);
-	if (!buf) {
-		dev_warn(&chan->par->pci_dev->dev, "Out of memory!\n");
-		return NULL;
+	for (i = 0; i < 3; i++) {
+		if (!par->chan[i].par)
+			continue;
+		i2c_del_adapter(&par->chan[i].adapter);
+		par->chan[i].par = NULL;
 	}
-	msgs[1].buf = buf;
-
-	if (i2c_transfer(&chan->adapter, msgs, 2) == 2)
-		return buf;
-	dev_dbg(&chan->par->pci_dev->dev, "Unable to read EDID block.\n");
-	kfree(buf);
-	return NULL;
 }
 
 int nvidia_probe_i2c_connector(struct fb_info *info, int conn, u8 **out_edid)
 {
 	struct nvidia_par *par = info->par;
 	u8 *edid = NULL;
-	int i;
 
-	for (i = 0; i < 3; i++) {
-		/* Do the real work */
-		edid = nvidia_do_probe_i2c_edid(&par->chan[conn - 1]);
-		if (edid)
-			break;
-	}
+	if (par->chan[conn - 1].par)
+		edid = fb_ddc_read(&par->chan[conn - 1].adapter);
 
 	if (!edid && conn == 1) {
 		/* try to get from firmware */
diff --git a/drivers/video/nvidia/nv_local.h b/drivers/video/nvidia/nv_local.h
index e009d242ea10..68e508daa417 100644
--- a/drivers/video/nvidia/nv_local.h
+++ b/drivers/video/nvidia/nv_local.h
@@ -73,9 +73,9 @@
 #define NVDmaNext(par, data) \
      NV_WR32(&(par)->dmaBase[(par)->dmaCurrent++], 0, (data))
 
-#define NVDmaStart(par, tag, size) {          \
+#define NVDmaStart(info, par, tag, size) {    \
      if((par)->dmaFree <= (size))             \
-        NVDmaWait(par, size);                 \
+        NVDmaWait(info, size);                \
      NVDmaNext(par, ((size) << 18) | (tag));  \
      (par)->dmaFree -= ((size) + 1);          \
 }
diff --git a/drivers/video/nvidia/nv_setup.c b/drivers/video/nvidia/nv_setup.c
index eab3e282a4de..707e2c8a13ed 100644
--- a/drivers/video/nvidia/nv_setup.c
+++ b/drivers/video/nvidia/nv_setup.c
@@ -261,7 +261,7 @@ static void nv10GetConfig(struct nvidia_par *par)
 	}
 #endif
 
-	dev = pci_find_slot(0, 1);
+	dev = pci_get_bus_and_slot(0, 1);
 	if ((par->Chipset & 0xffff) == 0x01a0) {
 		int amt = 0;
 
@@ -276,6 +276,7 @@ static void nv10GetConfig(struct nvidia_par *par)
 		par->RamAmountKBytes =
 		    (NV_RD32(par->PFB, 0x020C) & 0xFFF00000) >> 10;
 	}
+	pci_dev_put(dev);
 
 	par->CrystalFreqKHz = (NV_RD32(par->PEXTDEV, 0x0000) & (1 << 6)) ?
 	    14318 : 13500;
@@ -656,7 +657,7 @@ int NVCommonSetup(struct fb_info *info)
 	par->LVDS = 0;
 	if (par->FlatPanel && par->twoHeads) {
 		NV_WR32(par->PRAMDAC0, 0x08B0, 0x00010004);
-		if (par->PRAMDAC0[0x08b4] & 1)
+		if (NV_RD32(par->PRAMDAC0, 0x08b4) & 1)
 			par->LVDS = 1;
 		printk("nvidiafb: Panel is %s\n", par->LVDS ? "LVDS" : "TMDS");
 	}
diff --git a/drivers/video/nvidia/nv_type.h b/drivers/video/nvidia/nv_type.h
index 86e65dea60d3..38f7cc0a2331 100644
--- a/drivers/video/nvidia/nv_type.h
+++ b/drivers/video/nvidia/nv_type.h
@@ -4,8 +4,9 @@
 #include <linux/fb.h>
 #include <linux/types.h>
 #include <linux/i2c.h>
-#include <linux/i2c-id.h>
 #include <linux/i2c-algo-bit.h>
+#include <linux/mutex.h>
+#include <video/vga.h>
 
 #define NV_ARCH_04  0x04
 #define NV_ARCH_10  0x10
@@ -94,13 +95,15 @@ struct riva_regs {
 struct nvidia_par {
 	RIVA_HW_STATE SavedReg;
 	RIVA_HW_STATE ModeReg;
+	RIVA_HW_STATE initial_state;
 	RIVA_HW_STATE *CurrentState;
+	struct vgastate vgastate;
+	struct mutex open_lock;
 	u32 pseudo_palette[16];
 	struct pci_dev *pci_dev;
 	u32 Architecture;
 	u32 CursorStart;
 	int Chipset;
-	int bus;
 	unsigned long FbAddress;
 	u8 __iomem *FbStart;
 	u32 FbMapSize;
@@ -143,6 +146,7 @@ struct nvidia_par {
 	int BlendingPossible;
 	u32 paletteEnabled;
 	u32 forceCRTC;
+	u32 open_count;
 	u8 DDCBase;
 #ifdef CONFIG_MTRR
 	struct {
diff --git a/drivers/video/nvidia/nvidia.c b/drivers/video/nvidia/nvidia.c
index b97ec6901263..7c36b5fe582e 100644
--- a/drivers/video/nvidia/nvidia.c
+++ b/drivers/video/nvidia/nvidia.c
@@ -200,7 +200,7 @@ static int nvidia_panel_tweak(struct nvidia_par *par,
    return tweak;
 }
 
-static void nvidia_vga_protect(struct nvidia_par *par, int on)
+static void nvidia_screen_off(struct nvidia_par *par, int on)
 {
 	unsigned char tmp;
 
@@ -649,7 +649,7 @@ static int nvidiafb_set_par(struct fb_info *info)
 		NVLockUnlock(par, 0);
 	}
 
-	nvidia_vga_protect(par, 1);
+	nvidia_screen_off(par, 1);
 
 	nvidia_write_regs(par, &par->ModeReg);
 	NVSetStartAddress(par, 0);
@@ -687,7 +687,7 @@ static int nvidiafb_set_par(struct fb_info *info)
 
 	par->cursor_reset = 1;
 
-	nvidia_vga_protect(par, 0);
+	nvidia_screen_off(par, 0);
 
 #ifdef CONFIG_BOOTX_TEXT
 	/* Update debug text engine */
@@ -696,6 +696,7 @@ static int nvidiafb_set_par(struct fb_info *info)
 			     info->var.bits_per_pixel, info->fix.line_length);
 #endif
 
+	NVLockUnlock(par, 0);
 	NVTRACE_LEAVE();
 	return 0;
 }
@@ -948,8 +949,80 @@ static int nvidiafb_blank(int blank, struct fb_info *info)
 	return 0;
 }
 
+/*
+ * Because the VGA registers are not mapped linearly in its MMIO space,
+ * restrict VGA register saving and restore to x86 only, where legacy VGA IO
+ * access is legal. Consequently, we must also check if the device is the
+ * primary display.
+ */
+#ifdef CONFIG_X86
+static void save_vga_x86(struct nvidia_par *par)
+{
+	struct resource *res= &par->pci_dev->resource[PCI_ROM_RESOURCE];
+
+	if (res && res->flags & IORESOURCE_ROM_SHADOW) {
+		memset(&par->vgastate, 0, sizeof(par->vgastate));
+		par->vgastate.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS |
+			VGA_SAVE_CMAP;
+		save_vga(&par->vgastate);
+	}
+}
+
+static void restore_vga_x86(struct nvidia_par *par)
+{
+	struct resource *res= &par->pci_dev->resource[PCI_ROM_RESOURCE];
+
+	if (res && res->flags & IORESOURCE_ROM_SHADOW)
+		restore_vga(&par->vgastate);
+}
+#else
+#define save_vga_x86(x) do {} while (0)
+#define restore_vga_x86(x) do {} while (0)
+#endif /* X86 */
+
+static int nvidiafb_open(struct fb_info *info, int user)
+{
+	struct nvidia_par *par = info->par;
+
+	mutex_lock(&par->open_lock);
+
+	if (!par->open_count) {
+		save_vga_x86(par);
+		nvidia_save_vga(par, &par->initial_state);
+	}
+
+	par->open_count++;
+	mutex_unlock(&par->open_lock);
+	return 0;
+}
+
+static int nvidiafb_release(struct fb_info *info, int user)
+{
+	struct nvidia_par *par = info->par;
+	int err = 0;
+
+	mutex_lock(&par->open_lock);
+
+	if (!par->open_count) {
+		err = -EINVAL;
+		goto done;
+	}
+
+	if (par->open_count == 1) {
+		nvidia_write_regs(par, &par->initial_state);
+		restore_vga_x86(par);
+	}
+
+	par->open_count--;
+done:
+	mutex_unlock(&par->open_lock);
+	return err;
+}
+
 static struct fb_ops nvidia_fb_ops = {
 	.owner          = THIS_MODULE,
+	.fb_open        = nvidiafb_open,
+	.fb_release     = nvidiafb_release,
 	.fb_check_var   = nvidiafb_check_var,
 	.fb_set_par     = nvidiafb_set_par,
 	.fb_setcolreg   = nvidiafb_setcolreg,
@@ -1207,7 +1280,7 @@ static int __devinit nvidiafb_probe(struct pci_dev *pd,
 
 	par = info->par;
 	par->pci_dev = pd;
-
+	mutex_init(&par->open_lock);
 	info->pixmap.addr = kzalloc(8 * 1024, GFP_KERNEL);
 
 	if (info->pixmap.addr == NULL)