diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 65b76ff..c87974d 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -2191,6 +2191,12 @@ struct intel_watermark_params {
 	unsigned long default_wm;
 	unsigned long guard_size;
 	unsigned long cacheline_size;
+	/* indicate whether the required entry is treated as the watermark.
+	 * If it is 1, it will use the required entries to set the watermark.
+	 * Otherwise we will subtract the required entry from the total size
+	 * to get the watermark.
+	 */
+	int	watermark_flag;
 };
 
 /* IGD has different values for various configs */
@@ -2228,6 +2234,7 @@ static struct intel_watermark_params g4x_wm_info = {
 	G4X_MAX_WM,
 	2,
 	G4X_FIFO_LINE_SIZE,
+	1,
 };
 static struct intel_watermark_params i945_wm_info = {
 	I945_FIFO_SIZE,
@@ -2295,7 +2302,16 @@ static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
 
 	DRM_DEBUG_KMS("FIFO entries required for mode: %d\n", entries_required);
 
-	wm_size = wm->fifo_size - (entries_required + wm->guard_size);
+	/*
+	 * When the watermark flag is non-zero, the required entry size will
+	 * be treated as the watermark.
+	 * Otherwise we will subtract the required entry size from the total
+	 * size to get the watermark.
+	 */
+	if (wm->watermark_flag)
+		wm_size = entries_required + wm->guard_size;
+	else
+		wm_size = wm->fifo_size - (entries_required + wm->guard_size);
 
 	DRM_DEBUG_KMS("FIFO watermark level: %d\n", wm_size);
 
@@ -2524,39 +2540,37 @@ static void g4x_update_wm(struct drm_device *dev,  int planea_clock,
 			  int planeb_clock, int sr_hdisplay, 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;
 	struct intel_watermark_params planea_params, planeb_params;
-	unsigned long line_time_us;
-	int sr_clock, sr_entries = 0, entries_required;
+	int sr_clock, srwm = 8;
 
 	/* 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
 	 */
-	entries_required = ((planea_clock / 1000) * pixel_size * latency_ns) /
-		1000;
-	entries_required /= G4X_FIFO_LINE_SIZE;
-	planea_wm = entries_required + planea_params.guard_size;
-
-	entries_required = ((planeb_clock / 1000) * pixel_size * latency_ns) /
-		1000;
-	entries_required /= G4X_FIFO_LINE_SIZE;
-	planeb_wm = entries_required + planeb_params.guard_size;
+	planea_wm = intel_calculate_wm(planea_clock, &planea_params,
+					pixel_size, latency_ns);
 
+	planeb_wm = intel_calculate_wm(planeb_clock, &planeb_params,
+					pixel_size, latency_ns);
+
+	/* the cursor watermark should also be calculated by using the function
+	 * of intel_calculate_wm.
+	 * But as we have no proper latency setting and total fifo size
+	 * for cursor, now the fixed value is used.
+	 * The cursor self-refresh watermark is 32.
+	 * The cursor A/B watermark is 16.
+	 * If it is incorrect, please fix me.
+	 */
 	cursora_wm = cursorb_wm = 16;
 	cursor_sr = 32;
 
-	DRM_DEBUG("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
+	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
 
 	/* Calc sr entries for one plane configs */
 	if (sr_hdisplay && (!planea_clock || !planeb_clock)) {
@@ -2564,26 +2578,22 @@ static void g4x_update_wm(struct drm_device *dev,  int planea_clock,
 		const static int sr_latency_ns = 12000;
 
 		sr_clock = planea_clock ? planea_clock : planeb_clock;
-		line_time_us = ((sr_hdisplay * 1000) / sr_clock);
 
-		/* 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);
+		srwm = intel_calculate_wm(sr_clock, &g4x_wm_info,
+						pixel_size, sr_latency_ns);
+
+		DRM_DEBUG_KMS("self-refresh watermark: %d\n", srwm);
 		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;
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: %d, B: %d, SR %d\n",
+		  planea_wm, planeb_wm, srwm);
 
-	I915_WRITE(DSPFW1, (sr_entries << DSPFW_SR_SHIFT) |
+	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) |
 		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
 		   (planeb_wm << DSPFW_PLANEB_SHIFT) | planea_wm);
-	I915_WRITE(DSPFW2, (I915_READ(DSPFW2) & DSPFW_CURSORA_MASK) |
+	/* Only update the cursor A watermark bits in FW2 */
+	I915_WRITE(DSPFW2, (I915_READ(DSPFW2) & ~DSPFW_CURSORA_MASK) |
 		   (cursora_wm << DSPFW_CURSORA_SHIFT));
 	/* HPLL off in SR has some issues on G4x... disable it */
 	I915_WRITE(DSPFW3, (I915_READ(DSPFW3) & ~DSPFW_HPLL_SR_EN) |
@@ -2594,26 +2604,20 @@ static void i965_update_wm(struct drm_device *dev, int planea_clock,
 			   int planeb_clock, int sr_hdisplay, int pixel_size)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long line_time_us;
-	int sr_clock, sr_entries, srwm = 1;
+	int sr_clock, srwm = 8, cursor_srwm;
 
+	cursor_srwm = 16;
 	/* Calc sr entries for one plane configs */
 	if (sr_hdisplay && (!planea_clock || !planeb_clock)) {
 		/* self-refresh has much higher latency */
 		const static int sr_latency_ns = 12000;
 
 		sr_clock = planea_clock ? planea_clock : planeb_clock;
-		line_time_us = ((sr_hdisplay * 1000) / sr_clock);
-
-		/* 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(sr_clock, &i945_wm_info,
+						pixel_size, sr_latency_ns);
+		DRM_DEBUG_KMS("self-refresh watermark: %d\n", srwm);
+
 		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN);
 	}
 
@@ -2624,6 +2628,8 @@ static void i965_update_wm(struct drm_device *dev, int planea_clock,
 	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) | (8 << 16) | (8 << 8) |
 		   (8 << 0));
 	I915_WRITE(DSPFW2, (8 << 8) | (8 << 0));
+	/* Write the cursor self-refresh watermark */
+	I915_WRITE(DSPFW3, (cursor_srwm << 24));
 }
 
 static void i9xx_update_wm(struct drm_device *dev, int planea_clock,
@@ -2632,11 +2638,10 @@ static void i9xx_update_wm(struct drm_device *dev, int planea_clock,
 	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 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_entries = 0;
+	struct intel_watermark_params planea_params, planeb_params, sr_params;
+	int sr_clock;
 
 	/* Create copies of the base settings for each pipe */
 	if (IS_I965GM(dev) || IS_I945GM(dev))
@@ -2646,9 +2651,7 @@ static void i9xx_update_wm(struct drm_device *dev, int planea_clock,
 	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);
@@ -2672,16 +2675,10 @@ static void i9xx_update_wm(struct drm_device *dev, int planea_clock,
 		const static int sr_latency_ns = 6000;
 
 		sr_clock = planea_clock ? planea_clock : planeb_clock;
-		line_time_us = ((sr_hdisplay * 1000) / sr_clock);
-
-		/* 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_KMS("self-refresh entries: %d\n", sr_entries);
-		srwm = total_size - sr_entries;
-		if (srwm < 0)
-			srwm = 1;
+
+		srwm = intel_calculate_wm(sr_clock, &sr_params,
+						pixel_size, sr_latency_ns);
+		DRM_DEBUG_KMS("self-refresh watermark: %d\n", srwm);
 		I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN | (srwm & 0x3f));
 	}
 
