diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index e23e3c8..23898cf 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -340,7 +340,7 @@
 	unsigned long  fint_min, fint_max;
 	unsigned long lpdiv_max;
 
-	int num_data_lanes;
+	unsigned num_lanes_supported;
 
 	unsigned scp_clk_refcount;
 };
@@ -2080,19 +2080,8 @@
 	return 0;
 }
 
-/* Number of data lanes present on DSI interface */
-static inline int dsi_get_num_data_lanes(struct platform_device *dsidev)
-{
-	/* DSI on OMAP3 doesn't have register DSI_GNQ, set number
-	 * of data lanes as 2 by default */
-	if (dss_has_feature(FEAT_DSI_GNQ))
-		return REG_GET(dsidev, DSI_GNQ, 11, 9);	/* NB_DATA_LANES */
-	else
-		return 2;
-}
-
-/* Number of data lanes used by the dss device */
-static inline int dsi_get_num_data_lanes_dssdev(struct omap_dss_device *dssdev)
+/* Number of lanes used by the dss device */
+static inline int dsi_get_num_lanes_used(struct omap_dss_device *dssdev)
 {
 	int num_data_lanes = 0;
 
@@ -2105,7 +2094,7 @@
 	if (dssdev->phy.dsi.data4_lane != 0)
 		num_data_lanes++;
 
-	return num_data_lanes;
+	return num_data_lanes + 1;
 }
 
 static unsigned dsi_get_line_buf_size(struct platform_device *dsidev)
@@ -2143,7 +2132,7 @@
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	u32 r;
-	int num_data_lanes_dssdev = dsi_get_num_data_lanes_dssdev(dssdev);
+	int num_lanes_used = dsi_get_num_lanes_used(dssdev);
 
 	int clk_lane   = dssdev->phy.dsi.clk_lane;
 	int data1_lane = dssdev->phy.dsi.data1_lane;
@@ -2159,14 +2148,14 @@
 	r = FLD_MOD(r, data1_pol, 7, 7);
 	r = FLD_MOD(r, data2_lane, 10, 8);
 	r = FLD_MOD(r, data2_pol, 11, 11);
-	if (num_data_lanes_dssdev > 2) {
+	if (num_lanes_used > 3) {
 		int data3_lane  = dssdev->phy.dsi.data3_lane;
 		int data3_pol  = dssdev->phy.dsi.data3_pol;
 
 		r = FLD_MOD(r, data3_lane, 14, 12);
 		r = FLD_MOD(r, data3_pol, 15, 15);
 	}
-	if (num_data_lanes_dssdev > 3) {
+	if (num_lanes_used > 4) {
 		int data4_lane  = dssdev->phy.dsi.data4_lane;
 		int data4_pol  = dssdev->phy.dsi.data4_pol;
 
@@ -2298,7 +2287,7 @@
 	int data4_pol  = dssdev->phy.dsi.data4_pol;
 
 	u32 l = 0;
-	u8 lptxscp_start = dsi->num_data_lanes == 2 ? 22 : 26;
+	u8 lptxscp_start = dsi->num_lanes_supported == 3 ? 22 : 26;
 
 	if (lanes & DSI_CLK_P)
 		l |= 1 << ((clk_lane - 1) * 2 + (clk_pol ? 0 : 1));
@@ -2418,7 +2407,7 @@
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int r;
-	int num_data_lanes_dssdev = dsi_get_num_data_lanes_dssdev(dssdev);
+	int num_lanes_used = dsi_get_num_lanes_used(dssdev);
 	u32 l;
 
 	DSSDBGF();
@@ -2469,10 +2458,10 @@
 		 * manually.
 		 */
 
-		if (num_data_lanes_dssdev > 2)
+		if (num_lanes_used > 3)
 			lane_mask |= DSI_DATA3_P;
 
-		if (num_data_lanes_dssdev > 3)
+		if (num_lanes_used > 4)
 			lane_mask |= DSI_DATA4_P;
 
 		dsi_cio_enable_lane_override(dssdev, lane_mask);
@@ -3817,6 +3806,7 @@
 	unsigned enter_hs_mode_lat, exit_hs_mode_lat;
 	unsigned ths_eot;
 	unsigned offset_ddr_clk;
+	int ndl = dsi_get_num_lanes_used(dssdev) - 1;
 	u32 r;
 
 	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
@@ -4611,9 +4601,9 @@
 		dsi->vdds_dsi_reg = vdds_dsi;
 	}
 
-	if (dsi_get_num_data_lanes_dssdev(dssdev) > dsi->num_data_lanes) {
-		DSSERR("DSI%d can't support more than %d data lanes\n",
-			dsi_module + 1, dsi->num_data_lanes);
+	if (dsi_get_num_lanes_used(dssdev) > dsi->num_lanes_supported) {
+		DSSERR("DSI%d can't support more than %d lanes\n",
+			dsi_module + 1, dsi->num_lanes_supported);
 		return -EINVAL;
 	}
 
@@ -4843,7 +4833,13 @@
 	dev_dbg(&dsidev->dev, "OMAP DSI rev %d.%d\n",
 	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
 
-	dsi->num_data_lanes = dsi_get_num_data_lanes(dsidev);
+	/* DSI on OMAP3 doesn't have register DSI_GNQ, set number
+	 * of data to 3 by default */
+	if (dss_has_feature(FEAT_DSI_GNQ))
+		/* NB_DATA_LANES */
+		dsi->num_lanes_supported = 1 + REG_GET(dsidev, DSI_GNQ, 11, 9);
+	else
+		dsi->num_lanes_supported = 3;
 
 	dsi_runtime_put(dsidev);