msm: mdss: dp: add support for automated audio tests

On receiving audio test request from sink, parse audio parameters
and send notifications to audio modules to start audio transmission.

CRs-Fixed: 1109812
Change-Id: Id17d82c5b9e1c4bf453f1f1421d2025b32aa410a
Signed-off-by: Tatenda Chipeperekwa <tatendac@codeaurora.org>

4 files changed, 391 insertions, 22 deletions

diff --git a/drivers/video/fbdev/msm/mdss_dp.c b/drivers/video/fbdev/msm/mdss_dp.c
index 9553b7a5cefc..095a9a6fb44b 100644
--- a/drivers/video/fbdev/msm/mdss_dp.c
+++ b/drivers/video/fbdev/msm/mdss_dp.c
@@ -84,6 +84,12 @@ static inline bool mdss_dp_is_downstream_port_status_changed(
 	return dp->link_status.downstream_port_status_changed;
 }
 
+static inline bool mdss_dp_is_audio_pattern_requested(
+		struct mdss_dp_drv_pdata *dp)
+{
+	return (dp->test_data.test_requested & TEST_AUDIO_PATTERN);
+}
+
 static inline bool mdss_dp_is_link_training_requested(
 		struct mdss_dp_drv_pdata *dp)
 {
@@ -93,7 +99,8 @@ static inline bool mdss_dp_is_link_training_requested(
 static inline bool mdss_dp_is_video_pattern_requested(
 		struct mdss_dp_drv_pdata *dp)
 {
-	return (dp->test_data.test_requested == TEST_VIDEO_PATTERN);
+	return (dp->test_data.test_requested & TEST_VIDEO_PATTERN)
+		&& !(dp->test_data.test_requested & TEST_AUDIO_DISABLED_VIDEO);
 }
 
 static inline bool mdss_dp_is_phy_test_pattern_requested(
@@ -1574,8 +1581,11 @@ static int mdss_dp_send_cable_notification(
 
 	flags |= MSM_EXT_DISP_HPD_VIDEO;
 
-	if (!mdss_dp_is_dvi_mode(dp))
+	if (!mdss_dp_is_dvi_mode(dp) || dp->audio_test_req) {
+		dp->audio_test_req = false;
+
 		flags |= MSM_EXT_DISP_HPD_AUDIO;
+	}
 
 	if (dp->ext_audio_data.intf_ops.hpd)
 		ret = dp->ext_audio_data.intf_ops.hpd(dp->ext_pdev,
@@ -3165,6 +3175,48 @@ static int mdss_dp_process_phy_test_pattern_request(
 }
 
 /**
+ * mdss_dp_process_audio_pattern_request() - process new audio pattern request
+ * @dp: Display Port Driver data
+ *
+ * This function will handle a new audio pattern request that is initiated by
+ * the sink. This is acheieved by sending the necessary secondary data packets
+ * to the sink. It is expected that any simulatenous requests for video
+ * patterns will be handled before the audio pattern is sent to the sink.
+ */
+static int mdss_dp_process_audio_pattern_request(struct mdss_dp_drv_pdata *dp)
+{
+	if (!mdss_dp_is_audio_pattern_requested(dp))
+		return -EINVAL;
+
+	pr_debug("sampling_rate=%s, channel_count=%d, pattern_type=%s\n",
+		mdss_dp_get_audio_sample_rate(
+			dp->test_data.test_audio_sampling_rate),
+		dp->test_data.test_audio_channel_count,
+		mdss_dp_get_audio_test_pattern(
+			dp->test_data.test_audio_pattern_type));
+
+	pr_debug("audio_period: ch1=0x%x, ch2=0x%x, ch3=0x%x, ch4=0x%x\n",
+		dp->test_data.test_audio_period_ch_1,
+		dp->test_data.test_audio_period_ch_2,
+		dp->test_data.test_audio_period_ch_3,
+		dp->test_data.test_audio_period_ch_4);
+
+	pr_debug("audio_period: ch5=0x%x, ch6=0x%x, ch7=0x%x, ch8=0x%x\n",
+		dp->test_data.test_audio_period_ch_5,
+		dp->test_data.test_audio_period_ch_6,
+		dp->test_data.test_audio_period_ch_7,
+		dp->test_data.test_audio_period_ch_8);
+
+	if (dp->ext_audio_data.intf_ops.hpd)
+		dp->ext_audio_data.intf_ops.hpd(dp->ext_pdev,
+			dp->ext_audio_data.type, 1, MSM_EXT_DISP_HPD_AUDIO);
+
+	dp->audio_test_req = true;
+
+	return 0;
+}
+
+/**
  * mdss_dp_process_downstream_port_status_change() - process port status changes
  * @dp: Display Port Driver data
  *
@@ -3223,7 +3275,7 @@ static int mdss_dp_process_video_pattern_request(struct mdss_dp_drv_pdata *dp)
 	bool ov_res = false;
 
 	if (!mdss_dp_is_video_pattern_requested(dp))
-		return -EINVAL;
+		goto end;
 
 	pr_info("%s: bit depth=%d(%d bpp) pattern=%s\n",
 		mdss_dp_get_test_name(TEST_VIDEO_PATTERN),
@@ -3249,9 +3301,14 @@ static int mdss_dp_process_video_pattern_request(struct mdss_dp_drv_pdata *dp)
 
 	mdss_dp_link_maintenance(dp, lt_needed);
 
+	if (mdss_dp_is_audio_pattern_requested(dp))
+		goto end;
+
 	mdss_dp_send_test_response(dp);
 
 	return 0;
+end:
+	return -EINVAL;
 }
 
 /**
@@ -3294,6 +3351,10 @@ static int mdss_dp_process_hpd_irq_high(struct mdss_dp_drv_pdata *dp)
 	if (!ret)
 		goto exit;
 
+	ret = mdss_dp_process_audio_pattern_request(dp);
+	if (!ret)
+		goto exit;
+
 	pr_debug("done\n");
 
 exit:
diff --git a/drivers/video/fbdev/msm/mdss_dp.h b/drivers/video/fbdev/msm/mdss_dp.h
index 57bfdd366012..bf74a8a4d7df 100644
--- a/drivers/video/fbdev/msm/mdss_dp.h
+++ b/drivers/video/fbdev/msm/mdss_dp.h
@@ -284,6 +284,17 @@ struct dpcd_test_request {
 	u32 test_v_height;
 	u32 test_rr_d;
 	u32 test_rr_n;
+	u32 test_audio_sampling_rate;
+	u32 test_audio_channel_count;
+	u32 test_audio_pattern_type;
+	u32 test_audio_period_ch_1;
+	u32 test_audio_period_ch_2;
+	u32 test_audio_period_ch_3;
+	u32 test_audio_period_ch_4;
+	u32 test_audio_period_ch_5;
+	u32 test_audio_period_ch_6;
+	u32 test_audio_period_ch_7;
+	u32 test_audio_period_ch_8;
 	u32 response;
 };
 
@@ -439,6 +450,7 @@ struct mdss_dp_drv_pdata {
 	bool sink_info_read;
 	bool hpd;
 	bool psm_enabled;
+	bool audio_test_req;
 
 	/* dp specific */
 	unsigned char *base;
@@ -548,6 +560,55 @@ enum dp_lane_count {
 	DP_LANE_COUNT_4	= 4,
 };
 
+enum audio_pattern_type {
+	AUDIO_TEST_PATTERN_OPERATOR_DEFINED	= 0x00,
+	AUDIO_TEST_PATTERN_SAWTOOTH		= 0x01,
+};
+
+static inline char *mdss_dp_get_audio_test_pattern(u32 pattern)
+{
+	switch (pattern) {
+	case AUDIO_TEST_PATTERN_OPERATOR_DEFINED:
+		return DP_ENUM_STR(AUDIO_TEST_PATTERN_OPERATOR_DEFINED);
+	case AUDIO_TEST_PATTERN_SAWTOOTH:
+		return DP_ENUM_STR(AUDIO_TEST_PATTERN_SAWTOOTH);
+	default:
+		return "unknown";
+	}
+}
+
+enum audio_sample_rate {
+	AUDIO_SAMPLE_RATE_32_KHZ	= 0x00,
+	AUDIO_SAMPLE_RATE_44_1_KHZ	= 0x01,
+	AUDIO_SAMPLE_RATE_48_KHZ	= 0x02,
+	AUDIO_SAMPLE_RATE_88_2_KHZ	= 0x03,
+	AUDIO_SAMPLE_RATE_96_KHZ	= 0x04,
+	AUDIO_SAMPLE_RATE_176_4_KHZ	= 0x05,
+	AUDIO_SAMPLE_RATE_192_KHZ	= 0x06,
+};
+
+static inline char *mdss_dp_get_audio_sample_rate(u32 rate)
+{
+	switch (rate) {
+	case AUDIO_SAMPLE_RATE_32_KHZ:
+		return DP_ENUM_STR(AUDIO_SAMPLE_RATE_32_KHZ);
+	case AUDIO_SAMPLE_RATE_44_1_KHZ:
+		return DP_ENUM_STR(AUDIO_SAMPLE_RATE_44_1_KHZ);
+	case AUDIO_SAMPLE_RATE_48_KHZ:
+		return DP_ENUM_STR(AUDIO_SAMPLE_RATE_48_KHZ);
+	case AUDIO_SAMPLE_RATE_88_2_KHZ:
+		return DP_ENUM_STR(AUDIO_SAMPLE_RATE_88_2_KHZ);
+	case AUDIO_SAMPLE_RATE_96_KHZ:
+		return DP_ENUM_STR(AUDIO_SAMPLE_RATE_96_KHZ);
+	case AUDIO_SAMPLE_RATE_176_4_KHZ:
+		return DP_ENUM_STR(AUDIO_SAMPLE_RATE_176_4_KHZ);
+	case AUDIO_SAMPLE_RATE_192_KHZ:
+		return DP_ENUM_STR(AUDIO_SAMPLE_RATE_192_KHZ);
+	default:
+		return "unknown";
+	}
+}
+
 enum phy_test_pattern {
 	PHY_TEST_PATTERN_NONE,
 	PHY_TEST_PATTERN_D10_2_NO_SCRAMBLING,
@@ -648,6 +709,8 @@ enum test_type {
 	TEST_VIDEO_PATTERN	= 0x2,
 	PHY_TEST_PATTERN	= 0x8,
 	TEST_EDID_READ		= 0x4,
+	TEST_AUDIO_PATTERN		= 32,
+	TEST_AUDIO_DISABLED_VIDEO	= 64,
 };
 
 static inline char *mdss_dp_get_test_name(u32 test_requested)
@@ -657,6 +720,7 @@ static inline char *mdss_dp_get_test_name(u32 test_requested)
 	case TEST_VIDEO_PATTERN:	return DP_ENUM_STR(TEST_VIDEO_PATTERN);
 	case PHY_TEST_PATTERN:		return DP_ENUM_STR(PHY_TEST_PATTERN);
 	case TEST_EDID_READ:		return DP_ENUM_STR(TEST_EDID_READ);
+	case TEST_AUDIO_PATTERN:	return DP_ENUM_STR(TEST_AUDIO_PATTERN);
 	default:			return "unknown";
 	}
 }
diff --git a/drivers/video/fbdev/msm/mdss_dp_aux.c b/drivers/video/fbdev/msm/mdss_dp_aux.c
index bacf7bc46890..ca07e80d6613 100644
--- a/drivers/video/fbdev/msm/mdss_dp_aux.c
+++ b/drivers/video/fbdev/msm/mdss_dp_aux.c
@@ -1295,6 +1295,236 @@ static void dp_sink_parse_sink_count(struct mdss_dp_drv_pdata *ep)
 			ep->sink_count.count, ep->sink_count.cp_ready);
 }
 
+static int dp_get_test_period(struct mdss_dp_drv_pdata *ep, int const addr)
+{
+	int ret = 0;
+	char *bp;
+	char data;
+	struct edp_buf *rp;
+	int rlen;
+	int const test_parameter_len = 0x1;
+	int const max_audio_period = 0xA;
+
+	/* TEST_AUDIO_PERIOD_CH_XX */
+	rlen = dp_aux_read_buf(ep, addr, test_parameter_len, 0);
+	if (rlen < test_parameter_len) {
+		pr_err("failed to read test_audio_period (0x%x)\n", addr);
+		ret = -EINVAL;
+		goto exit;
+	}
+	rp = &ep->rxp;
+	bp = rp->data;
+	data = *bp++;
+
+	/* Period - Bits 3:0 */
+	data = data & 0xF;
+	if ((int)data > max_audio_period) {
+		pr_err("invalid test_audio_period_ch_1 = 0x%x\n", data);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	ret = data;
+
+exit:
+	return ret;
+}
+
+static int dp_parse_audio_channel_test_period(struct mdss_dp_drv_pdata *ep)
+{
+	int ret = 0;
+	int const test_audio_period_ch_1_addr = 0x273;
+	int const test_audio_period_ch_2_addr = 0x274;
+	int const test_audio_period_ch_3_addr = 0x275;
+	int const test_audio_period_ch_4_addr = 0x276;
+	int const test_audio_period_ch_5_addr = 0x277;
+	int const test_audio_period_ch_6_addr = 0x278;
+	int const test_audio_period_ch_7_addr = 0x279;
+	int const test_audio_period_ch_8_addr = 0x27A;
+
+	/* TEST_AUDIO_PERIOD_CH_1 (Byte 0x273) */
+	ret = dp_get_test_period(ep, test_audio_period_ch_1_addr);
+	if (ret == -EINVAL)
+		goto exit;
+
+	ep->test_data.test_audio_period_ch_1 = ret;
+	pr_debug("test_audio_period_ch_1 = 0x%x\n", ret);
+
+	/* TEST_AUDIO_PERIOD_CH_2 (Byte 0x274) */
+	ret = dp_get_test_period(ep, test_audio_period_ch_2_addr);
+	if (ret == -EINVAL)
+		goto exit;
+
+	ep->test_data.test_audio_period_ch_2 = ret;
+	pr_debug("test_audio_period_ch_2 = 0x%x\n", ret);
+
+	/* TEST_AUDIO_PERIOD_CH_3 (Byte 0x275) */
+	ret = dp_get_test_period(ep, test_audio_period_ch_3_addr);
+	if (ret == -EINVAL)
+		goto exit;
+
+	ep->test_data.test_audio_period_ch_3 = ret;
+	pr_debug("test_audio_period_ch_3 = 0x%x\n", ret);
+
+	/* TEST_AUDIO_PERIOD_CH_4 (Byte 0x276) */
+	ret = dp_get_test_period(ep, test_audio_period_ch_4_addr);
+	if (ret == -EINVAL)
+		goto exit;
+
+	ep->test_data.test_audio_period_ch_4 = ret;
+	pr_debug("test_audio_period_ch_4 = 0x%x\n", ret);
+
+	/* TEST_AUDIO_PERIOD_CH_5 (Byte 0x277) */
+	ret = dp_get_test_period(ep, test_audio_period_ch_5_addr);
+	if (ret == -EINVAL)
+		goto exit;
+
+	ep->test_data.test_audio_period_ch_5 = ret;
+	pr_debug("test_audio_period_ch_5 = 0x%x\n", ret);
+
+	/* TEST_AUDIO_PERIOD_CH_6 (Byte 0x278) */
+	ret = dp_get_test_period(ep, test_audio_period_ch_6_addr);
+	if (ret == -EINVAL)
+		goto exit;
+
+	ep->test_data.test_audio_period_ch_6 = ret;
+	pr_debug("test_audio_period_ch_6 = 0x%x\n", ret);
+
+	/* TEST_AUDIO_PERIOD_CH_7 (Byte 0x279) */
+	ret = dp_get_test_period(ep, test_audio_period_ch_7_addr);
+	if (ret == -EINVAL)
+		goto exit;
+
+	ep->test_data.test_audio_period_ch_7 = ret;
+	pr_debug("test_audio_period_ch_7 = 0x%x\n", ret);
+
+	/* TEST_AUDIO_PERIOD_CH_8 (Byte 0x27A) */
+	ret = dp_get_test_period(ep, test_audio_period_ch_8_addr);
+	if (ret == -EINVAL)
+		goto exit;
+
+	ep->test_data.test_audio_period_ch_8 = ret;
+	pr_debug("test_audio_period_ch_8 = 0x%x\n", ret);
+
+
+exit:
+	return ret;
+}
+
+static int dp_parse_audio_pattern_type(struct mdss_dp_drv_pdata *ep)
+{
+	int ret = 0;
+	char *bp;
+	char data;
+	struct edp_buf *rp;
+	int rlen;
+	int const test_parameter_len = 0x1;
+	int const test_audio_pattern_type_addr = 0x272;
+	int const max_audio_pattern_type = 0x1;
+
+	/* Read the requested audio pattern type (Byte 0x272). */
+	rlen = dp_aux_read_buf(ep, test_audio_pattern_type_addr,
+			test_parameter_len, 0);
+	if (rlen < test_parameter_len) {
+		pr_err("failed to read test audio mode data\n");
+		ret = -EINVAL;
+		goto exit;
+	}
+	rp = &ep->rxp;
+	bp = rp->data;
+	data = *bp++;
+
+	/* Audio Pattern Type - Bits 7:0 */
+	if ((int)data > max_audio_pattern_type) {
+		pr_err("invalid audio pattern type = 0x%x\n", data);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	ep->test_data.test_audio_pattern_type = data;
+	pr_debug("audio pattern type = %s\n",
+			mdss_dp_get_audio_test_pattern(data));
+
+exit:
+	return ret;
+}
+
+static int dp_parse_audio_mode(struct mdss_dp_drv_pdata *ep)
+{
+	int ret = 0;
+	char *bp;
+	char data;
+	struct edp_buf *rp;
+	int rlen;
+	int const test_parameter_len = 0x1;
+	int const test_audio_mode_addr = 0x271;
+	int const max_audio_sampling_rate = 0x6;
+	int const max_audio_channel_count = 0x8;
+	int sampling_rate = 0x0;
+	int channel_count = 0x0;
+
+	/* Read the requested audio mode (Byte 0x271). */
+	rlen = dp_aux_read_buf(ep, test_audio_mode_addr,
+			test_parameter_len, 0);
+	if (rlen < test_parameter_len) {
+		pr_err("failed to read test audio mode data\n");
+		ret = -EINVAL;
+		goto exit;
+	}
+	rp = &ep->rxp;
+	bp = rp->data;
+	data = *bp++;
+
+	/* Sampling Rate - Bits 3:0 */
+	sampling_rate = data & 0xF;
+	if (sampling_rate > max_audio_sampling_rate) {
+		pr_err("sampling rate (0x%x) greater than max (0x%x)\n",
+				sampling_rate, max_audio_sampling_rate);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	/* Channel Count - Bits 7:4 */
+	channel_count = ((data & 0xF0) >> 4) + 1;
+	if (channel_count > max_audio_channel_count) {
+		pr_err("channel_count (0x%x) greater than max (0x%x)\n",
+				channel_count, max_audio_channel_count);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	ep->test_data.test_audio_sampling_rate = sampling_rate;
+	ep->test_data.test_audio_channel_count = channel_count;
+	pr_debug("sampling_rate = %s, channel_count = 0x%x\n",
+		mdss_dp_get_audio_sample_rate(sampling_rate), channel_count);
+
+exit:
+	return ret;
+}
+
+/**
+ * dp_parse_audio_pattern_params() - parses audio pattern parameters from DPCD
+ * @ep: Display Port Driver data
+ *
+ * Returns 0 if it successfully parses the audio test pattern parameters.
+ */
+static int dp_parse_audio_pattern_params(struct mdss_dp_drv_pdata *ep)
+{
+	int ret = 0;
+
+	ret = dp_parse_audio_mode(ep);
+	if (ret)
+		goto exit;
+
+	ret = dp_parse_audio_pattern_type(ep);
+	if (ret)
+		goto exit;
+
+	ret = dp_parse_audio_channel_test_period(ep);
+
+exit:
+	return ret;
+}
 /**
  * dp_parse_phy_test_params() - parses the phy test parameters
  * @ep: Display Port Driver data
@@ -1572,6 +1802,19 @@ exit:
 	return ret;
 }
 
+/**
+ * mdss_dp_is_video_audio_test_requested() - checks for audio/video test request
+ * @test: test requested by the sink
+ *
+ * Returns true if the requested test is a permitted audio/video test.
+ */
+static bool mdss_dp_is_video_audio_test_requested(u32 test)
+{
+	return (test == TEST_VIDEO_PATTERN) ||
+		(test == (TEST_AUDIO_PATTERN | TEST_VIDEO_PATTERN)) ||
+		(test == TEST_AUDIO_PATTERN) ||
+		(test == (TEST_AUDIO_PATTERN | TEST_AUDIO_DISABLED_VIDEO));
+}
 
 /**
  * dp_is_test_supported() - checks if test requested by sink is supported
@@ -1582,9 +1825,9 @@ exit:
 static bool dp_is_test_supported(u32 test_requested)
 {
 	return (test_requested == TEST_LINK_TRAINING) ||
-		(test_requested == TEST_VIDEO_PATTERN) ||
 		(test_requested == TEST_EDID_READ) ||
-		(test_requested == PHY_TEST_PATTERN);
+		(test_requested == PHY_TEST_PATTERN) ||
+		mdss_dp_is_video_audio_test_requested(test_requested);
 }
 
 /**
@@ -1646,26 +1889,27 @@ static void dp_sink_parse_test_request(struct mdss_dp_drv_pdata *ep)
 		return;
 	}
 
-	pr_debug("%s requested\n", mdss_dp_get_test_name(data));
+	pr_debug("%s (0x%x) requested\n", mdss_dp_get_test_name(data), data);
 	ep->test_data.test_requested = data;
 
-	switch (ep->test_data.test_requested) {
-	case PHY_TEST_PATTERN:
+	if (ep->test_data.test_requested == PHY_TEST_PATTERN) {
 		ret = dp_parse_phy_test_params(ep);
 		if (ret)
-			break;
-	case TEST_LINK_TRAINING:
+			goto end;
 		ret = dp_parse_link_training_params(ep);
-		break;
-	case TEST_VIDEO_PATTERN:
-		ret = dp_parse_video_pattern_params(ep);
-		break;
-	default:
-		pr_debug("test 0x%x not supported\n",
-				ep->test_data.test_requested);
-		return;
 	}
 
+	if (ep->test_data.test_requested == TEST_LINK_TRAINING)
+		ret = dp_parse_link_training_params(ep);
+
+	if (mdss_dp_is_video_audio_test_requested(
+				ep->test_data.test_requested)) {
+		ret = dp_parse_video_pattern_params(ep);
+		if (ret)
+			goto end;
+		ret = dp_parse_audio_pattern_params(ep);
+	}
+end:
 	/* clear the test request IRQ */
 	buf[0] = 1;
 	dp_aux_write_buf(ep, test_request_addr, buf, 1, 0);
diff --git a/drivers/video/fbdev/msm/mdss_dp_util.c b/drivers/video/fbdev/msm/mdss_dp_util.c
index 92182e9d61b2..10812070807c 100644
--- a/drivers/video/fbdev/msm/mdss_dp_util.c
+++ b/drivers/video/fbdev/msm/mdss_dp_util.c
@@ -1160,7 +1160,7 @@ static void mdss_dp_audio_setup_audio_stream_sdp(struct dss_io_data *ctrl_io,
 
 	/* Config header and parity byte 2 */
 	value = readl_relaxed(ctrl_io->base + MMSS_DP_AUDIO_STREAM_1);
-	new_value = 0x0;
+	new_value = value;
 	parity_byte = mdss_dp_calculate_parity_byte(new_value);
 	value |= ((new_value << HEADER_BYTE_2_BIT)
 			| (parity_byte << PARITY_BYTE_2_BIT));
@@ -1208,7 +1208,7 @@ static void mdss_dp_audio_setup_audio_timestamp_sdp(struct dss_io_data *ctrl_io)
 
 	/* Config header and parity byte 3 */
 	value = readl_relaxed(ctrl_io->base + MMSS_DP_AUDIO_TIMESTAMP_1);
-	new_value = (0x0 | (0x12 << 2));
+	new_value = (0x0 | (0x11 << 2));
 	parity_byte = mdss_dp_calculate_parity_byte(new_value);
 	value |= ((new_value << HEADER_BYTE_3_BIT)
 			| (parity_byte << PARITY_BYTE_3_BIT));
@@ -1245,7 +1245,7 @@ static void mdss_dp_audio_setup_audio_infoframe_sdp(struct dss_io_data *ctrl_io)
 
 	/* Config header and parity byte 3 */
 	value = readl_relaxed(ctrl_io->base + MMSS_DP_AUDIO_INFOFRAME_1);
-	new_value = (0x0 | (0x12 << 2));
+	new_value = (0x0 | (0x11 << 2));
 	parity_byte = mdss_dp_calculate_parity_byte(new_value);
 	value |= ((new_value << HEADER_BYTE_3_BIT)
 			| (parity_byte << PARITY_BYTE_3_BIT));
@@ -1395,7 +1395,7 @@ void mdss_dp_set_safe_to_exit_level(struct dss_io_data *ctrl_io,
 	}
 
 	mainlink_levels = readl_relaxed(ctrl_io->base + DP_MAINLINK_LEVELS);
-	mainlink_levels &= 0xFF0;
+	mainlink_levels &= 0xFE0;
 	mainlink_levels |= safe_to_exit_level;
 
 	pr_debug("mainlink_level = 0x%x, safe_to_exit_level = 0x%x\n",