HDMI stereo support - kernel code changes

Signed-off-by: Chong Zhang <chzhang@nvidia.com>
Original-Change-Id: I53eea3b14852aecd589dfbd0461258e064835cc9
Reviewed-on: http://git-master/r/11405
Reviewed-by: Daniel Willemsen <dwillemsen@nvidia.com>

Rebase-Id: R4c2c1bba6eeeee16cf3d99061717c236c695eaf7

7 files changed, 207 insertions, 3 deletions

diff --git a/arch/arm/mach-tegra/include/mach/dc.h b/arch/arm/mach-tegra/include/mach/dc.h
index d4056c967555..bd9afa58be89 100644
--- a/arch/arm/mach-tegra/include/mach/dc.h
+++ b/arch/arm/mach-tegra/include/mach/dc.h
@@ -37,6 +37,7 @@ struct tegra_dc_mode {
 	int	v_active;
 	int	h_front_porch;
 	int	v_front_porch;
+	int	stereo_mode;
 	u32	flags;
 };
 
diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index 590310155bb1..34048e2b124a 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -1241,7 +1241,7 @@ void fb_var_to_videomode(struct fb_videomode *mode,
 	mode->upper_margin = var->upper_margin;
 	mode->lower_margin = var->lower_margin;
 	mode->sync = var->sync;
-	mode->vmode = var->vmode & FB_VMODE_MASK;
+	mode->vmode = var->vmode & (FB_VMODE_MASK | FB_VMODE_STEREO_MASK);
 	mode->flag = FB_MODE_IS_FROM_VAR;
 	mode->refresh = 0;
 
@@ -1286,7 +1286,7 @@ void fb_videomode_to_var(struct fb_var_screeninfo *var,
 	var->hsync_len = mode->hsync_len;
 	var->vsync_len = mode->vsync_len;
 	var->sync = mode->sync;
-	var->vmode = mode->vmode & FB_VMODE_MASK;
+	var->vmode = mode->vmode & (FB_VMODE_MASK | FB_VMODE_STEREO_MASK);
 }
 
 /**
diff --git a/drivers/video/tegra/dc/edid.c b/drivers/video/tegra/dc/edid.c
index 47f05e6ac31c..83c9f88f825d 100644
--- a/drivers/video/tegra/dc/edid.c
+++ b/drivers/video/tegra/dc/edid.c
@@ -32,6 +32,7 @@ struct tegra_edid {
 
 	u8			*data;
 	unsigned		len;
+	u8			support_stereo;
 };
 
 #if defined(DEBUG) || defined(CONFIG_DEBUG_FS)
@@ -162,10 +163,84 @@ int tegra_edid_read_block(struct tegra_edid *edid, int block, u8 *data)
 	return 0;
 }
 
+int tegra_edid_parse_ext_block(u8 *raw, int idx, struct tegra_edid *edid)
+{
+	u8 *ptr;
+	u8 tmp;
+	u8 code;
+	int len;
+
+	ptr = &raw[4];
+
+	while (ptr < &raw[idx]) {
+		tmp = *ptr;
+		len = tmp & 0x1f;
+
+		/* HDMI Specification v1.4a, section 8.3.2:
+		 * see Table 8-16 for HDMI VSDB format.
+		 * data blocks have tags in top 3 bits:
+		 * tag code 2: video data block
+		 * tag code 3: vendor specific data block
+		 */
+		code = (tmp >> 5) & 0x3;
+		switch (code) {
+		/* case 2 is commented out for now */
+		case 3:
+		{
+			int j = 0;
+
+			if ((len >= 8) &&
+				(ptr[1] == 0x03) &&
+				(ptr[2] == 0x0c) &&
+				(ptr[3] == 0)) {
+				j = 8;
+				tmp = ptr[j++];
+				/* HDMI_Video_present? */
+				if (tmp & 0x20) {
+					/* Latency_Fields_present? */
+					if (tmp & 0x80)
+						j += 2;
+					/* I_Latency_Fields_present? */
+					if (tmp & 0x40)
+						j += 2;
+					/* 3D_present? */
+					if (j <= len && (ptr[j] & 0x80))
+						edid->support_stereo = 1;
+				}
+			}
+
+			len++;
+			ptr += len; /* adding the header */
+			break;
+		}
+		default:
+			len++; /* len does not include header */
+			ptr += len;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+int tegra_edid_mode_support_stereo(struct fb_videomode *mode)
+{
+	if (!mode)
+		return 0;
+
+	if (mode->xres == 1280 && mode->yres == 720 && mode->refresh == 60)
+		return 1;
+
+	if (mode->xres == 1280 && mode->yres == 720 && mode->refresh == 50)
+		return 1;
+
+	return 0;
+}
 
 int tegra_edid_get_monspecs(struct tegra_edid *edid, struct fb_monspecs *specs)
 {
 	int i;
+	int j;
 	int ret;
 	int extension_blocks;
 
@@ -185,8 +260,21 @@ int tegra_edid_get_monspecs(struct tegra_edid *edid, struct fb_monspecs *specs)
 		if (ret < 0)
 			break;
 
-		if (edid->data[i * 128] == 0x2)
+		if (edid->data[i * 128] == 0x2) {
 			fb_edid_add_monspecs(edid->data + i * 128, specs);
+
+			tegra_edid_parse_ext_block(edid->data + i * 128,
+					edid->data[i * 128 + 2], edid);
+
+			if (edid->support_stereo) {
+				for (j = 0; j < specs->modedb_len; j++) {
+					if (tegra_edid_mode_support_stereo(
+						&specs->modedb[j]))
+						specs->modedb[j].vmode |=
+						FB_VMODE_STEREO_FRAME_PACK;
+				}
+			}
+		}
 	}
 
 	edid->len = i * 128;
diff --git a/drivers/video/tegra/dc/hdmi.c b/drivers/video/tegra/dc/hdmi.c
index ba2dc19072f9..bbab48260739 100644
--- a/drivers/video/tegra/dc/hdmi.c
+++ b/drivers/video/tegra/dc/hdmi.c
@@ -81,6 +81,22 @@ const struct fb_videomode tegra_dc_hdmi_supported_modes[] = {
 		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 	},
 
+	/* 1280x720p 60hz: EIA/CEA-861-B Format 4 (Stereo)*/
+	{
+		.xres =		1280,
+		.yres =		720,
+		.pixclock =	KHZ2PICOS(74250),
+		.hsync_len =	40,	/* h_sync_width */
+		.vsync_len =	5,	/* v_sync_width */
+		.left_margin =	220,	/* h_back_porch */
+		.upper_margin =	20,	/* v_back_porch */
+		.right_margin =	110,	/* h_front_porch */
+		.lower_margin =	5,	/* v_front_porch */
+		.vmode = FB_VMODE_NONINTERLACED |
+				 FB_VMODE_STEREO_FRAME_PACK,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+
 	/* 720x480p 59.94hz: EIA/CEA-861-B Formats 2 & 3 */
 	{
 		.xres =		720,
@@ -875,6 +891,38 @@ static void tegra_dc_hdmi_setup_avi_infoframe(struct tegra_dc *dc, bool dvi)
 			  HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_CTRL);
 }
 
+static void tegra_dc_hdmi_setup_stereo_infoframe(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	struct hdmi_stereo_infoframe stereo;
+	u32 val;
+
+	if (!dc->mode.stereo_mode) {
+		val  = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+		val &= ~GENERIC_CTRL_ENABLE;
+		tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+		return;
+	}
+
+	memset(&stereo, 0x0, sizeof(stereo));
+
+	stereo.regid0 = 0x03;
+	stereo.regid1 = 0x0c;
+	stereo.regid2 = 0x00;
+	stereo.hdmi_video_format = 2; /* 3D_Structure present */
+	stereo._3d_structure = 0; /* frame packing */
+
+	tegra_dc_hdmi_write_infopack(dc, HDMI_NV_PDISP_HDMI_GENERIC_HEADER,
+					HDMI_INFOFRAME_TYPE_VENDOR,
+					HDMI_VENDOR_VERSION,
+					&stereo, 6);
+
+	val  = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+	val |= GENERIC_CTRL_ENABLE;
+
+	tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+}
+
 static void tegra_dc_hdmi_setup_audio_infoframe(struct tegra_dc *dc, bool dvi)
 {
 	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
@@ -998,6 +1046,7 @@ static void tegra_dc_hdmi_enable(struct tegra_dc *dc)
 
 	tegra_dc_hdmi_setup_avi_infoframe(dc, hdmi->dvi);
 	tegra_dc_hdmi_setup_audio_infoframe(dc, hdmi->dvi);
+	tegra_dc_hdmi_setup_stereo_infoframe(dc);
 
 	/* TMDS CONFIG */
 	pll0 = 0x200033f;
diff --git a/drivers/video/tegra/dc/hdmi.h b/drivers/video/tegra/dc/hdmi.h
index a2fa3482f2b9..f726f41f8dbc 100644
--- a/drivers/video/tegra/dc/hdmi.h
+++ b/drivers/video/tegra/dc/hdmi.h
@@ -180,10 +180,41 @@ struct hdmi_audio_infoframe {
 #define HDMI_AUDIO_CXT_HE_AAC_V2	0x2
 #define HDMI_AUDIO_CXT_MPEG_SURROUND	0x3
 
+/* all fields little endian */
+struct hdmi_stereo_infoframe {
+	/* PB0 */
+	u8		csum;
+
+	/* PB1 */
+	u8		regid0;
+
+	/* PB2 */
+	u8		regid1;
+
+	/* PB3 */
+	u8		regid2;
+
+	/* PB4 */
+	unsigned	res1:5;
+	unsigned	hdmi_video_format:3;
+
+	/* PB5 */
+	unsigned	res2:4;
+	unsigned	_3d_structure:4;
+
+	/* PB6*/
+	unsigned	res3:4;
+	unsigned	_3d_ext_data:4;
+
+} __attribute__((packed));
+
+#define HDMI_VENDOR_VERSION 0x01
+
 struct tegra_dc_hdmi_data;
 
 unsigned long tegra_hdmi_readl(struct tegra_dc_hdmi_data *hdmi,
 				unsigned long reg);
 void tegra_hdmi_writel(struct tegra_dc_hdmi_data *hdmi,
 				unsigned long val, unsigned long reg);
+
 #endif
diff --git a/drivers/video/tegra/fb.c b/drivers/video/tegra/fb.c
index 8ac5c1bf7217..72583977fa8f 100644
--- a/drivers/video/tegra/fb.c
+++ b/drivers/video/tegra/fb.c
@@ -193,6 +193,21 @@ static int tegra_fb_set_par(struct fb_info *info)
 		mode.v_active = info->mode->yres;
 		mode.h_front_porch = info->mode->right_margin;
 		mode.v_front_porch = info->mode->lower_margin;
+		/*
+		 * only enable stereo if the mode supports it and
+		 * client requests it
+		 */
+		mode.stereo_mode = !!(var->vmode & info->mode->vmode &
+					FB_VMODE_STEREO_FRAME_PACK);
+
+		if (mode.stereo_mode) {
+			mode.pclk *= 2;
+			/* total v_active = yres*2 + activespace */
+			mode.v_active = info->mode->yres*2 +
+					info->mode->vsync_len +
+					info->mode->upper_margin +
+					info->mode->lower_margin;
+		}
 
 		mode.flags = 0;
 
@@ -600,6 +615,17 @@ static int tegra_fb_ioctl(struct fb_info *info, unsigned int cmd, unsigned long
 					 &var, sizeof(var)))
 				return -EFAULT;
 			i++;
+
+			if (var.vmode & FB_VMODE_STEREO_MASK) {
+				if (i >= modedb.modedb_len)
+					break;
+				var.vmode &= ~FB_VMODE_STEREO_MASK;
+				if (copy_to_user(
+					(void __user *)&modedb.modedb[i],
+					 &var, sizeof(var)))
+					return -EFAULT;
+				i++;
+			}
 		}
 		modedb.modedb_len = i;
 
diff --git a/include/linux/fb.h b/include/linux/fb.h
index f0532fca9cff..bb565a4c2e1c 100644
--- a/include/linux/fb.h
+++ b/include/linux/fb.h
@@ -231,6 +231,15 @@ struct fb_bitfield {
 #define FB_FLAG_PIXEL_REPEAT	256
 
 /*
+ * Stereo modes
+ */
+#define FB_VMODE_STEREO_NONE        0x00000000  /* not stereo */
+#define FB_VMODE_STEREO_FRAME_PACK  0x01000000  /* frame packing */
+#define FB_VMODE_STEREO_TOP_BOTTOM  0x02000000  /* top-bottom */
+#define FB_VMODE_STEREO_LEFT_RIGHT  0x04000000  /* left-right */
+#define FB_VMODE_STEREO_MASK        0xFF000000
+
+/*
  * Display rotation support
  */
 #define FB_ROTATE_UR      0