diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 847006c..07ab3ba 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -1846,6 +1846,15 @@
 #define PINEVIEW_CURSOR_DFT_WM	0
 #define PINEVIEW_CURSOR_GUARD_WM	5
 
+
+#define I965GM_DISPLAY_FIFO	512 /* in 64byte unit */
+#define I965GM_FIFO_LINE_SIZE	64
+#define I965GM_MAX_WM		0x1ff
+#define I965GM_DFT_WM		0x3f
+
+#define I965GM_CURSOR_FIFO	64 /* in 64bytes unit */
+#define I965GM_CURSOR_MAX_WM	32
+#define I965GM_CURSOR_DFT_WM	8
 /*
  * The two pipe frame counter registers are not synchronized, so
  * reading a stable value is somewhat tricky. The following code
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 59ccc7f..1ce4b0a 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -2197,6 +2197,42 @@ static struct intel_watermark_params g4x_wm_info = {
 	WM_USE_ENTRY_COUNT,
 };
 
+static struct intel_watermark_params g4x_self_wm_info = {
+	I965GM_DISPLAY_FIFO,
+	I965GM_MAX_WM,
+	I965GM_DFT_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+	WM_USE_ENTRY_COUNT,
+};
+
+static struct intel_watermark_params g4x_self_cursor_info = {
+	I965GM_CURSOR_FIFO,
+	I965GM_CURSOR_MAX_WM,
+	I965GM_CURSOR_DFT_WM,
+	2,
+	G4X_FIFO_LINE_SIZE,
+	WM_USE_ENTRY_COUNT,
+};
+
+static struct intel_watermark_params gm965_self_wm_info = {
+	I965GM_DISPLAY_FIFO,
+	I965GM_MAX_WM,
+	I965GM_DFT_WM,
+	2,
+	I965GM_FIFO_LINE_SIZE,
+	WM_USE_OFFSET,
+};
+
+static struct intel_watermark_params gm965_self_cursor_info = {
+	I965GM_CURSOR_FIFO,
+	I965GM_CURSOR_MAX_WM,
+	I965GM_CURSOR_DFT_WM,
+	2,
+	I965GM_FIFO_LINE_SIZE,
+	WM_USE_OFFSET,
+};
+
 static struct intel_watermark_params i945_wm_info = {
 	I945_FIFO_SIZE,
 	I915_MAX_WM,
@@ -2602,39 +2638,31 @@ static void g4x_update_wm(struct drm_device *dev,
 				int pixel_size)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int total_size, cacheline_size;
-	int planea_wm, planeb_wm, cursora_wm, cursorb_wm, cursor_sr;
+	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
 	struct intel_watermark_params planea_params, planeb_params;
-	unsigned long line_time_us;
-	int sr_clock, sr_entries = 0, entries_required, sr_hdisplay;
+	int sr_entries = 32, cursor_sr;
 
 	/* Create copies of the base settings for each pipe */
 	planea_params = planeb_params = g4x_wm_info;
 
-	/* Grab a couple of global values before we overwrite them */
-	total_size = planea_params.fifo_size;
-	cacheline_size = planea_params.cacheline_size;
-
 	/*
 	 * Note: we need to make sure we don't overflow for various clock &
 	 * latency values.
 	 * clocks go from a few thousand to several hundred thousand.
 	 * latency is usually a few thousand
 	 */
-	if (crtc_modea->clock) {
-		entries_required = ((crtc_modea->clock / 1000) * pixel_size *
-				latency_ns) / 1000;
-		entries_required /= G4X_FIFO_LINE_SIZE;
-		planea_wm = entries_required + planea_params.guard_size;
-	} else
+	if (crtc_modea->clock)
+		planea_wm = intel_calculate_wm(&planea_params, crtc_modea,
+				pixel_size, latency_ns,
+				WM_USE_NORMAL, WM_TYPE_PLANE);
+	else
 		planea_wm = 16;
 
-	if (crtc_modeb->clock) {
-		entries_required = ((crtc_modeb->clock / 1000) * pixel_size *
-				latency_ns) / 1000;
-		entries_required /= G4X_FIFO_LINE_SIZE;
-		planeb_wm = entries_required + planeb_params.guard_size;
-	} else
+	if (crtc_modeb->clock)
+		planeb_wm = intel_calculate_wm(&planeb_params, crtc_modeb,
+				pixel_size, latency_ns,
+				WM_USE_NORMAL, WM_TYPE_PLANE);
+	else
 		planeb_wm = 16;
 
 	cursora_wm = cursorb_wm = 16;
@@ -2644,39 +2672,41 @@ static void g4x_update_wm(struct drm_device *dev,
 
 	/* Calc sr entries for one plane configs */
 	if (!crtc_modea->clock || !crtc_modeb->clock) {
+		struct drm_display_mode *crtc_mode;
 		/* self-refresh has much higher latency */
 		static const int sr_latency_ns = 12000;
-		if (crtc_modea->clock) {
-			sr_clock = crtc_modea->clock;
-			sr_hdisplay = crtc_modea->hdisplay;
-		} else {
-			sr_clock = crtc_modeb->clock;
-			sr_hdisplay = crtc_modeb->hdisplay;
-		}
-		line_time_us = ((sr_hdisplay * 1000) / sr_clock);
+		if (crtc_modea->clock)
+			crtc_mode = crtc_modea;
+		else
+			crtc_mode = crtc_modeb;
 
-		/* Use ns/us then divide to preserve precision */
-		sr_entries = (((sr_latency_ns / line_time_us) + 1) *
-			      pixel_size * sr_hdisplay) / 1000;
-		sr_entries = roundup(sr_entries / cacheline_size, 1);
-		DRM_DEBUG("self-refresh entries: %d\n", sr_entries);
+		sr_entries = intel_calculate_wm(&g4x_self_wm_info, crtc_mode,
+				pixel_size, sr_latency_ns,
+				WM_USE_LARGE_BUFFER, WM_TYPE_PLANE);
+		cursor_sr = intel_calculate_wm(&g4x_self_cursor_info, crtc_mode,
+				pixel_size, sr_latency_ns,
+				WM_USE_LARGE_BUFFER, WM_TYPE_CURSOR);
+
+		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);
 
-	planea_wm &= 0x3f;
-	planeb_wm &= 0x3f;
-
 	I915_WRITE(DSPFW1, (sr_entries << DSPFW_SR_SHIFT) |
 		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
 		   (planeb_wm << DSPFW_PLANEB_SHIFT) | planea_wm);
-	I915_WRITE(DSPFW2, (I915_READ(DSPFW2) & DSPFW_CURSORA_MASK) |
-		   (cursora_wm << DSPFW_CURSORA_SHIFT));
+	/* 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) |
-		   (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
+	I915_WRITE(DSPFW3, (I915_READ(DSPFW3) & ~DSPFW_HPLL_SR_EN));
 }
 
 static void i965_update_wm(struct drm_device *dev,
@@ -2685,33 +2715,28 @@ static void i965_update_wm(struct drm_device *dev,
 				int pixel_size)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long line_time_us;
-	int sr_clock, sr_entries, srwm = 1, sr_hdisplay;
+	int srwm = 16, cursor_sr = 16;
 
 	/* Calc sr entries for one plane configs */
 	if (!crtc_modea->clock || !crtc_modeb->clock) {
+		struct drm_display_mode *crtc_mode;
 		/* self-refresh has much higher latency */
 		static const int sr_latency_ns = 12000;
 
-		if (crtc_modea->clock) {
-			sr_hdisplay = crtc_modea->hdisplay;
-			sr_clock = crtc_modea->clock;
-		} else {
-			sr_hdisplay = crtc_modeb->hdisplay;
-			sr_clock = crtc_modeb->clock;
-		}
-
-		line_time_us = ((sr_hdisplay * 1000) / sr_clock);
+		if (crtc_modea->clock)
+			crtc_mode = crtc_modea;
+		else
+			crtc_mode = crtc_modeb;
 
-		/* Use ns/us then divide to preserve precision */
-		sr_entries = (((sr_latency_ns / line_time_us) + 1) *
-			      pixel_size * sr_hdisplay) / 1000;
-		sr_entries = roundup(sr_entries / I915_FIFO_LINE_SIZE, 1);
-		DRM_DEBUG("self-refresh entries: %d\n", sr_entries);
-		srwm = I945_FIFO_SIZE - sr_entries;
-		if (srwm < 0)
-			srwm = 1;
-		srwm &= 0x3f;
+		srwm = intel_calculate_wm(&gm965_self_wm_info, crtc_mode,
+				pixel_size, sr_latency_ns,
+				WM_USE_LARGE_BUFFER, WM_TYPE_PLANE);
+		cursor_sr = intel_calculate_wm(&gm965_self_cursor_info,
+				crtc_mode,
+				pixel_size, sr_latency_ns,
+				WM_USE_LARGE_BUFFER, WM_TYPE_CURSOR);
+		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);
 	}
 
@@ -2722,6 +2747,8 @@ static void i965_update_wm(struct drm_device *dev,
 	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) | (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));
 }
 
 static void i9xx_update_wm(struct drm_device *dev,
@@ -2732,11 +2759,9 @@ static void i9xx_update_wm(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint32_t fwater_lo;
 	uint32_t fwater_hi;
-	int total_size, cacheline_size, cwm, srwm = 1;
 	int planea_wm, planeb_wm;
-	struct intel_watermark_params planea_params, planeb_params;
-	unsigned long line_time_us;
-	int sr_clock, sr_hdisplay, sr_entries = 0;
+	struct intel_watermark_params planea_params, planeb_params, sr_params;
+	int sr_entries = 16, cwm;
 
 	/* Create copies of the base settings for each pipe */
 	if (IS_I965GM(dev) || IS_I945GM(dev))
@@ -2746,9 +2771,7 @@ static void i9xx_update_wm(struct drm_device *dev,
 	else
 		planea_params = planeb_params = i855_wm_info;
 
-	/* Grab a couple of global values before we overwrite them */
-	total_size = planea_params.fifo_size;
-	cacheline_size = planea_params.cacheline_size;
+	sr_params = planea_params;
 
 	/* Update per-plane FIFO sizes */
 	planea_params.fifo_size = dev_priv->display.get_fifo_size(dev, 0);
@@ -2767,7 +2790,6 @@ static void i9xx_update_wm(struct drm_device *dev,
 						WM_USE_NORMAL, WM_TYPE_PLANE);
 	else
 		planeb_wm = 8;
-	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
 
 	/*
 	 * Overlay gets an aggressive default since video jitter is bad.
@@ -2777,32 +2799,24 @@ static void i9xx_update_wm(struct drm_device *dev,
 	/* Calc sr entries for one plane configs */
 	if (HAS_FW_BLC(dev) &&
 	    (!crtc_modea->clock || !crtc_modeb->clock)) {
+		struct drm_display_mode *crtc_mode;
 		/* self-refresh has much higher latency */
 		static const int sr_latency_ns = 6000;
 
-		if (crtc_modea->clock) {
-			sr_clock = crtc_modea->clock;
-			sr_hdisplay = crtc_modea->hdisplay;
-		} else {
-			sr_clock = crtc_modeb->clock;
-			sr_hdisplay = crtc_modeb->hdisplay;
-		}
-
-		line_time_us = ((sr_hdisplay * 1000) / sr_clock);
+		if (crtc_modea->clock)
+			crtc_mode = crtc_modea;
+		else
+			crtc_mode = crtc_modeb;
 
-		/* Use ns/us then divide to preserve precision */
-		sr_entries = (((sr_latency_ns / line_time_us) + 1) *
-			      pixel_size * sr_hdisplay) / 1000;
-		sr_entries = roundup(sr_entries / cacheline_size, 1);
+		sr_entries = intel_calculate_wm(&sr_params, crtc_mode,
+					pixel_size, sr_latency_ns,
+					WM_USE_LARGE_BUFFER, WM_TYPE_PLANE);
 		DRM_DEBUG_KMS("self-refresh entries: %d\n", sr_entries);
-		srwm = total_size - sr_entries;
-		if (srwm < 0)
-			srwm = 1;
-		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN | (srwm & 0x3f));
+		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, srwm);
+		  planea_wm, planeb_wm, cwm, sr_entries);
 
 	fwater_lo = ((planeb_wm & 0x3f) << 16) | (planea_wm & 0x3f);
 	fwater_hi = (cwm & 0x1f);
