diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 07ab3ba..f5d5d15 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -1813,12 +1813,14 @@
 #define   DSPFW_SR_SHIFT	23
 #define   DSPFW_CURSORB_SHIFT	16
 #define   DSPFW_PLANEB_SHIFT	8
+#define   DSPFW_SR_MASK		(0x1ff << 23)
 #define DSPFW2			0x70038
 #define   DSPFW_CURSORA_MASK	0x00003f00
 #define   DSPFW_CURSORA_SHIFT	8
 #define DSPFW3			0x7003c
 #define   DSPFW_HPLL_SR_EN	(1<<31)
 #define   DSPFW_CURSOR_SR_SHIFT	24
+#define   DSPFW_CURSOR_SR_MASK	(0x3f << 24)
 #define   PINEVIEW_SELF_REFRESH_EN	(1<<30)
 
 /* FIFO watermark sizes etc */
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 1ce4b0a..13689b8 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -2641,6 +2641,7 @@ static void g4x_update_wm(struct drm_device *dev,
 	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
 	struct intel_watermark_params planea_params, planeb_params;
 	int sr_entries = 32, cursor_sr;
+	u32 reg_value;
 
 	/* Create copies of the base settings for each pipe */
 	planea_params = planeb_params = g4x_wm_info;
@@ -2666,7 +2667,6 @@ static void g4x_update_wm(struct drm_device *dev,
 		planeb_wm = 16;
 
 	cursora_wm = cursorb_wm = 16;
-	cursor_sr = 32;
 
 	DRM_DEBUG("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
 
@@ -2687,24 +2687,33 @@ static void g4x_update_wm(struct drm_device *dev,
 				pixel_size, sr_latency_ns,
 				WM_USE_LARGE_BUFFER, WM_TYPE_CURSOR);
 
+		reg_value = I915_READ(DSPFW1);
+		reg_value &= ~DSPFW_SR_MASK;
+		reg_value |= (sr_entries << DSPFW_SR_SHIFT);
+		I915_WRITE(DSPFW1, reg_value);
+
+		reg_value = I915_READ(DSPFW3);
+		reg_value &= ~DSPFW_CURSOR_SR_MASK;
+		reg_value |= (cursor_sr << DSPFW_CURSOR_SR_SHIFT);
+		I915_WRITE(DSPFW3, reg_value);
+
 		DRM_DEBUG_KMS("self-refresh entries: display plane %d ",
 				"display cursor %d\n", sr_entries, cursor_sr);
 		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
 	}
 
-	DRM_DEBUG("Setting FIFO watermarks - A: %d, B: %d, SR %d\n",
-		  planea_wm, planeb_wm, sr_entries);
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d, B: %d\n",
+		  planea_wm, planeb_wm);
+
+	reg_value = I915_READ(DSPFW1);
+	reg_value &= DSPFW_SR_MASK;
+	reg_value |= (cursorb_wm << DSPFW_CURSORB_SHIFT) |
+			(planeb_wm << DSPFW_PLANEB_SHIFT) | planea_wm;
+	I915_WRITE(DSPFW1, reg_value);
 
-	I915_WRITE(DSPFW1, (sr_entries << DSPFW_SR_SHIFT) |
-		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
-		   (planeb_wm << DSPFW_PLANEB_SHIFT) | planea_wm);
 	/* Only update cursor A watermark in FW2 */
 	I915_WRITE(DSPFW2, (I915_READ(DSPFW2) & ~DSPFW_CURSORA_MASK) |
 			(cursora_wm << DSPFW_CURSORA_SHIFT));
-	/* Update the cursor self-refresh watermark */
-	I915_WRITE(DSPFW3, (I915_READ(DSPFW3) &
-			~(0x3f << DSPFW_CURSOR_SR_SHIFT)) |
-			(cursor_sr << DSPFW_CURSOR_SR_SHIFT));
 	/* HPLL off in SR has some issues on G4x... disable it */
 	I915_WRITE(DSPFW3, (I915_READ(DSPFW3) & ~DSPFW_HPLL_SR_EN));
 }
@@ -2716,6 +2725,7 @@ static void i965_update_wm(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int srwm = 16, cursor_sr = 16;
+	u32 reg_value;
 
 	/* Calc sr entries for one plane configs */
 	if (!crtc_modea->clock || !crtc_modeb->clock) {
@@ -2735,20 +2745,33 @@ static void i965_update_wm(struct drm_device *dev,
 				crtc_mode,
 				pixel_size, sr_latency_ns,
 				WM_USE_LARGE_BUFFER, WM_TYPE_CURSOR);
+
+		reg_value = I915_READ(DSPFW1);
+		reg_value &= ~DSPFW_SR_MASK;
+		reg_value |= (srwm << DSPFW_SR_SHIFT);
+		I915_WRITE(DSPFW1, reg_value);
+
+		reg_value = I915_READ(DSPFW3);
+		reg_value &= ~DSPFW_CURSOR_SR_MASK;
+		reg_value |= (cursor_sr << DSPFW_CURSOR_SR_SHIFT);
+		I915_WRITE(DSPFW3, reg_value);
+
 		DRM_DEBUG_KMS("self-refresh entries: display plane %d ",
 				"display cursor %d\n", srwm, cursor_sr);
 		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
 	}
 
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: 8, B: 8, C: 8, SR %d\n",
-		      srwm);
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: 8, B: 8, C: 8 \n");
 
 	/* 965 has limitations... */
-	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) | (8 << 16) | (8 << 8) |
-		   (8 << 0));
+	reg_value = I915_READ(DSPFW1);
+	reg_value &= DSPFW_SR_MASK;
+	I915_WRITE(DSPFW1, reg_value | (8 << 16) | (8 << 8) | (8 << 0));
+
 	I915_WRITE(DSPFW2, (8 << 8) | (8 << 0));
-	/* update the cursor self-refresh watermark */
-	I915_WRITE(DSPFW3, (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
+
+	/* Disable HPLL off in SR*/
+	I915_WRITE(DSPFW3, (I915_READ(DSPFW3) & ~DSPFW_HPLL_SR_EN));
 }
 
 static void i9xx_update_wm(struct drm_device *dev,
@@ -2815,8 +2838,8 @@ static void i9xx_update_wm(struct drm_device *dev,
 		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN | (sr_entries & 0x3f));
 	}
 
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d, B: %d, C: %d, SR %d\n",
-		  planea_wm, planeb_wm, cwm, sr_entries);
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d, B: %d, C: %d\n",
+		  planea_wm, planeb_wm, cwm);
 
 	fwater_lo = ((planeb_wm & 0x3f) << 16) | (planea_wm & 0x3f);
 	fwater_hi = (cwm & 0x1f);
