Index: src/radeon_driver.c
===================================================================
RCS file: /cvs/xorg/driver/xf86-video-ati/src/radeon_driver.c,v
retrieving revision 1.86
diff -p -u -r1.86 radeon_driver.c
--- src/radeon_driver.c	16 Feb 2006 23:27:44 -0000	1.86
+++ src/radeon_driver.c	17 Feb 2006 17:10:37 -0000
@@ -868,6 +868,9 @@ void RADEONWaitForVerticalSync(ScrnInfoP
     unsigned char *RADEONMMIO = info->MMIO;
     int            i;
 
+    if (INREG(RADEON_CRTC_GEN_CNTL) & RADEON_CRTC_DISP_REQ_EN_B)
+	return;
+
     /* Clear the CRTC_VBLANK_SAVE bit */
     OUTREG(RADEON_CRTC_STATUS, RADEON_CRTC_VBLANK_SAVE_CLEAR);
 
@@ -885,6 +888,9 @@ void RADEONWaitForVerticalSync2(ScrnInfo
     unsigned char *RADEONMMIO = info->MMIO;
     int            i;
 
+    if (INREG(RADEON_CRTC2_GEN_CNTL) & RADEON_CRTC2_DISP_REQ_EN_B)
+	return;
+
     /* Clear the CRTC2_VBLANK_SAVE bit */
     OUTREG(RADEON_CRTC2_STATUS, RADEON_CRTC2_VBLANK_SAVE_CLEAR);
 
@@ -5934,7 +5939,7 @@ static void RADEONRestoreMemMapRegisters
      */
     if (INREG(RADEON_MC_FB_LOCATION) != restore->mc_fb_location ||
 	INREG(RADEON_MC_AGP_LOCATION) != restore->mc_agp_location) {
-	CARD32 tmp;
+	CARD32 crtc_ext_cntl, crtc_gen_cntl, crtc2_gen_cntl=0, ov0_scale_cntl;
 
 	RADEONTRACE(("  Map Changed ! Applying ...\n"));
 
@@ -5944,21 +5949,24 @@ static void RADEONRestoreMemMapRegisters
 	RADEONWaitForIdleMMIO(pScrn);
 
 	/* Stop display & memory access */
-	tmp = INREG(RADEON_CRTC_EXT_CNTL);
-	OUTREG(RADEON_CRTC_EXT_CNTL, tmp | RADEON_CRTC_DISPLAY_DIS);
-	tmp = INREG(RADEON_CRTC_GEN_CNTL);
-	tmp &= ~RADEON_CRTC_CUR_EN;
-	tmp |= RADEON_CRTC_DISP_REQ_EN_B;
-	OUTREG(RADEON_CRTC_GEN_CNTL, tmp);
+	ov0_scale_cntl = INREG(RADEON_OV0_SCALE_CNTL);
+	OUTREG(RADEON_OV0_SCALE_CNTL, ov0_scale_cntl & ~RADEON_SCALER_ENABLE);
+	crtc_ext_cntl = INREG(RADEON_CRTC_EXT_CNTL);
+	OUTREG(RADEON_CRTC_EXT_CNTL, crtc_ext_cntl | RADEON_CRTC_DISPLAY_DIS);
+	crtc_gen_cntl = INREG(RADEON_CRTC_GEN_CNTL);
+	RADEONWaitForVerticalSync(pScrn);
+	OUTREG(RADEON_CRTC_GEN_CNTL, (crtc_gen_cntl & ~(RADEON_CRTC_CUR_EN | RADEON_CRTC_ICON_EN)) |
+				     RADEON_CRTC_DISP_REQ_EN_B);
 	if (info->HasCRTC2) {
-		tmp = INREG(RADEON_CRTC2_GEN_CNTL);
-		tmp &= ~RADEON_CRTC2_CUR_EN;
-		tmp |= RADEON_CRTC2_DISP_DIS | RADEON_CRTC2_DISP_REQ_EN_B;
-		OUTREG(RADEON_CRTC2_GEN_CNTL, tmp);
-	}
-	tmp = INREG(RADEON_OV0_SCALE_CNTL);
-	tmp &= ~RADEON_SCALER_ENABLE;
-	OUTREG(RADEON_OV0_SCALE_CNTL, tmp);
+	    crtc2_gen_cntl = INREG(RADEON_CRTC2_GEN_CNTL);
+	    RADEONWaitForVerticalSync2(pScrn);
+	    OUTREG(RADEON_CRTC2_GEN_CNTL, (crtc2_gen_cntl & ~(RADEON_CRTC2_CUR_EN | RADEON_CRTC2_ICON_EN)) |
+					  RADEON_CRTC2_DISP_REQ_EN_B);
+	}
+
+	/* Wait for MC idle */
+	while (!(INREG(RADEON_MC_STATUS) & RADEON_MC_IDLE))
+	    usleep(1);
 
 	/* Make sure the chip settles down and set new map*/ 
 	usleep(100000);
@@ -5973,7 +5981,8 @@ static void RADEONRestoreMemMapRegisters
     
     /* Restore base addresses */
     OUTREG(RADEON_DISPLAY_BASE_ADDR, restore->display_base_addr);
-    OUTREG(RADEON_DISPLAY2_BASE_ADDR, restore->display2_base_addr);
+    if (info->HasCRTC2)
+	OUTREG(RADEON_DISPLAY2_BASE_ADDR, restore->display2_base_addr);
     OUTREG(RADEON_OV0_BASE_ADDR, restore->ov0_base_addr);
 }
 
Index: src/radeon_reg.h
===================================================================
RCS file: /cvs/xorg/driver/xf86-video-ati/src/radeon_reg.h,v
retrieving revision 1.21
diff -p -u -r1.21 radeon_reg.h
--- src/radeon_reg.h	16 Feb 2006 23:27:44 -0000	1.21
+++ src/radeon_reg.h	17 Feb 2006 17:10:37 -0000
@@ -307,9 +307,9 @@
 #       define RADEON_CRTC_DBL_SCAN_EN      (1 <<  0)
 #       define RADEON_CRTC_INTERLACE_EN     (1 <<  1)
 #       define RADEON_CRTC_CSYNC_EN         (1 <<  4)
+#       define RADEON_CRTC_ICON_EN          (1 << 15)
 #       define RADEON_CRTC_CUR_EN           (1 << 16)
 #       define RADEON_CRTC_CUR_MODE_MASK    (7 << 17)
-#       define RADEON_CRTC_ICON_EN          (1 << 20)
 #       define RADEON_CRTC_EXT_DISP_EN      (1 << 24)
 #       define RADEON_CRTC_EN               (1 << 25)
 #       define RADEON_CRTC_DISP_REQ_EN_B    (1 << 26)
@@ -848,6 +848,8 @@
 
 #define RADEON_MAX_LATENCY                  0x0f3f /* PCI */
 #define RADEON_MC_AGP_LOCATION              0x014c
+#define RADEON_MC_STATUS                    0x0150
+#       define RADEON_MC_IDLE               (1 << 2)
 #define RADEON_MC_FB_LOCATION               0x0148
 #define RADEON_DISPLAY_BASE_ADDR            0x23c
 #define RADEON_DISPLAY2_BASE_ADDR           0x33c