From a9a965ddc7aaa9d2b7eda607b7b2d2b1f4ebf06d Mon Sep 17 00:00:00 2001
From: Tomi Valkeinen <tomi.valkeinen@ti.com>
Date: Wed, 15 Jun 2011 15:22:47 +0300
Subject: [PATCH] --- yaml --- r: 271375 b: refs/heads/master c:
 dc35835c6f4bf3f15b68c723c9b7540cf11b9ad6 h: refs/heads/master i:   271373:
 2f3a15d46588f34d4d1482c5a448a2c371c37d2c   271371:
 cff27fcc273dd7e156c1644fe4a1b05b4a1916bc   271367:
 fa7df9478d8e24da63b2b2506dc2304b398f9564   271359:
 475eba0b182f10222f01afb4e9d734671aca675a v: v3

---
 [refs]                              |  2 +-
 trunk/arch/arm/mach-omap2/display.c | 40 +++++++++++++++++++++++++++++
 2 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/[refs] b/[refs]
index 4a1580c70a9d..567375370942 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 5bc416cba15f43c799fc02727c6d6887f3e35a4e
+refs/heads/master: dc35835c6f4bf3f15b68c723c9b7540cf11b9ad6
diff --git a/trunk/arch/arm/mach-omap2/display.c b/trunk/arch/arm/mach-omap2/display.c
index 7c6011df80a8..62510ec863c6 100644
--- a/trunk/arch/arm/mach-omap2/display.c
+++ b/trunk/arch/arm/mach-omap2/display.c
@@ -27,6 +27,8 @@
 #include <plat/omap_device.h>
 #include <plat/omap-pm.h>
 
+#include "control.h"
+
 static struct platform_device omap_display_device = {
 	.name          = "omapdss",
 	.id            = -1,
@@ -74,13 +76,51 @@ static const struct omap_dss_hwmod_data omap4_dss_hwmod_data[] __initdata = {
 	{ "dss_hdmi", "omapdss_hdmi", -1 },
 };
 
+static int omap4_dsi_mux_pads(int dsi_id, unsigned lanes)
+{
+	u32 enable_mask, enable_shift;
+	u32 pipd_mask, pipd_shift;
+	u32 reg;
+
+	if (dsi_id == 0) {
+		enable_mask = OMAP4_DSI1_LANEENABLE_MASK;
+		enable_shift = OMAP4_DSI1_LANEENABLE_SHIFT;
+		pipd_mask = OMAP4_DSI1_PIPD_MASK;
+		pipd_shift = OMAP4_DSI1_PIPD_SHIFT;
+	} else if (dsi_id == 1) {
+		enable_mask = OMAP4_DSI2_LANEENABLE_MASK;
+		enable_shift = OMAP4_DSI2_LANEENABLE_SHIFT;
+		pipd_mask = OMAP4_DSI2_PIPD_MASK;
+		pipd_shift = OMAP4_DSI2_PIPD_SHIFT;
+	} else {
+		return -ENODEV;
+	}
+
+	reg = omap4_ctrl_pad_readl(OMAP4_CTRL_MODULE_PAD_CORE_CONTROL_DSIPHY);
+
+	reg &= ~enable_mask;
+	reg &= ~pipd_mask;
+
+	reg |= (lanes << enable_shift) & enable_mask;
+	reg |= (lanes << pipd_shift) & pipd_mask;
+
+	omap4_ctrl_pad_writel(reg, OMAP4_CTRL_MODULE_PAD_CORE_CONTROL_DSIPHY);
+
+	return 0;
+}
+
 static int omap_dsi_enable_pads(int dsi_id, unsigned lane_mask)
 {
+	if (cpu_is_omap44xx())
+		return omap4_dsi_mux_pads(dsi_id, lane_mask);
+
 	return 0;
 }
 
 static void omap_dsi_disable_pads(int dsi_id, unsigned lane_mask)
 {
+	if (cpu_is_omap44xx())
+		omap4_dsi_mux_pads(dsi_id, 0);
 }
 
 int __init omap_display_init(struct omap_dss_board_info *board_data)