1 files changed, 35 insertions, 32 deletions

diff --git a/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c b/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
index 21964ba..c220f01 100644
--- a/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
+++ b/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
@@ -187,41 +187,21 @@ static const u8 *mipi_exec_delay(struct intel_dsi *intel_dsi, const u8 *data)
 	return data;
 }
 
-static const u8 *mipi_exec_gpio(struct intel_dsi *intel_dsi, const u8 *data)
+static void vlv_exec_gpio(struct drm_i915_private *dev_priv,
+			  u8 gpio_source, u8 gpio_index, bool value)
 {
-	u8 gpio_source, gpio_index, action, port;
 	u16 pconf0, padval;
-	u32 val;
-	struct drm_device *dev = intel_dsi->base.base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (dev_priv->vbt.dsi.seq_version >= 3)
-		data++;
-
-	gpio_index = *data++;
-
-	/* gpio source in sequence v2 only */
-	if (dev_priv->vbt.dsi.seq_version == 2)
-		gpio_source = (*data >> 1) & 3;
-	else
-		gpio_source = 0;
-
-	/* pull up/down */
-	action = *data++ & 1;
+	u32 tmp;
+	u8 port;
 
 	if (gpio_index >= ARRAY_SIZE(vlv_gpio_table)) {
 		DRM_DEBUG_KMS("unknown gpio index %u\n", gpio_index);
-		goto out;
-	}
-
-	if (!IS_VALLEYVIEW(dev_priv)) {
-		DRM_DEBUG_KMS("GPIO element not supported on this platform\n");
-		goto out;
+		return;
 	}
 
 	if (dev_priv->vbt.dsi.seq_version >= 3) {
 		DRM_DEBUG_KMS("GPIO element v3 not supported\n");
-		goto out;
+		return;
 	} else {
 		if (gpio_source == 0) {
 			port = IOSF_PORT_GPIO_NC;
@@ -229,7 +209,7 @@ static const u8 *mipi_exec_gpio(struct intel_dsi *intel_dsi, const u8 *data)
 			port = IOSF_PORT_GPIO_SC;
 		} else {
 			DRM_DEBUG_KMS("unknown gpio source %u\n", gpio_source);
-			goto out;
+			return;
 		}
 	}
 
@@ -238,19 +218,42 @@ static const u8 *mipi_exec_gpio(struct intel_dsi *intel_dsi, const u8 *data)
 
 	mutex_lock(&dev_priv->sb_lock);
 	if (!vlv_gpio_table[gpio_index].init) {
-		/* program the function */
 		/* FIXME: remove constant below */
 		vlv_iosf_sb_write(dev_priv, port, pconf0, 0x2000CC00);
 		vlv_gpio_table[gpio_index].init = true;
 	}
 
-	val = 0x4 | action;
+	tmp = 0x4 | value;
+	vlv_iosf_sb_write(dev_priv, port, padval, tmp);
+	mutex_unlock(&dev_priv->sb_lock);
+}
+
+static const u8 *mipi_exec_gpio(struct intel_dsi *intel_dsi, const u8 *data)
+{
+	struct drm_device *dev = intel_dsi->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u8 gpio_source, gpio_index;
+	bool value;
+
+	if (dev_priv->vbt.dsi.seq_version >= 3)
+		data++;
+
+	gpio_index = *data++;
+
+	/* gpio source in sequence v2 only */
+	if (dev_priv->vbt.dsi.seq_version == 2)
+		gpio_source = (*data >> 1) & 3;
+	else
+		gpio_source = 0;
 
 	/* pull up/down */
-	vlv_iosf_sb_write(dev_priv, port, padval, val);
-	mutex_unlock(&dev_priv->sb_lock);
+	value = *data++ & 1;
+
+	if (IS_VALLEYVIEW(dev_priv))
+		vlv_exec_gpio(dev_priv, gpio_source, gpio_index, value);
+	else
+		DRM_DEBUG_KMS("GPIO element not supported on this platform\n");
 
-out:
 	return data;
 }