diff --git a/drivers/video/fbdev/msm/mdss_mdp.c b/drivers/video/fbdev/msm/mdss_mdp.c
index 1dae41391795..066ee193e165 100644
--- a/drivers/video/fbdev/msm/mdss_mdp.c
+++ b/drivers/video/fbdev/msm/mdss_mdp.c
@@ -3584,6 +3584,7 @@ static int mdss_mdp_cdm_addr_setup(struct mdss_data_type *mdata,
 		head[i].base = (mdata->mdss_io.base) + cdm_offsets[i];
 		atomic_set(&head[i].kref.refcount, 0);
 		mutex_init(&head[i].lock);
+		init_completion(&head[i].free_comp);
 		pr_debug("%s: cdm off (%d) = %pK\n", __func__, i, head[i].base);
 	}
 
diff --git a/drivers/video/fbdev/msm/mdss_mdp_cdm.c b/drivers/video/fbdev/msm/mdss_mdp_cdm.c
index e0bf1cdf1361..6cc848d93eb9 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_cdm.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_cdm.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -26,6 +26,8 @@ static u32 cdm_cdwn2_offsite_h_coeff[] = {0x000b0005, 0x01db01eb, 0x00e40046};
 static u32 cdm_cdwn2_cosite_v_coeff[] = {0x00080004};
 static u32 cdm_cdwn2_offsite_v_coeff[] = {0x00060002};
 
+#define VSYNC_TIMEOUT_US 16000
+
 /**
  * @mdss_mdp_cdm_alloc() - Allocates a cdm block by parsing the list of
  *			     available cdm blocks.
@@ -66,6 +68,7 @@ static void mdss_mdp_cdm_free(struct kref *kref)
 	if (!cdm)
 		return;
 
+	complete_all(&cdm->free_comp);
 	pr_debug("free cdm_num = %d\n", cdm->num);
 
 }
@@ -84,6 +87,28 @@ struct mdss_mdp_cdm *mdss_mdp_cdm_init(struct mdss_mdp_ctl *ctl, u32 intf_type)
 
 	cdm = mdss_mdp_cdm_alloc(mdata);
 
+	/**
+	 * give hdmi interface priority to alloc the cdm block. It will wait
+	 * for one vsync cycle to allow wfd to finish its job and try to reserve
+	 * the block the again.
+	 */
+	if (!cdm && (intf_type == MDP_CDM_CDWN_OUTPUT_HDMI)) {
+		/* always wait for first cdm block */
+		cdm = mdata->cdm_off;
+		if (cdm) {
+			reinit_completion(&cdm->free_comp);
+			/*
+			 * no need to check the return status of completion
+			 * timeout. Next cdm_alloc call will try to reserve
+			 * the cdm block and returns failure if allocation
+			 * fails.
+			 */
+			wait_for_completion_timeout(&cdm->free_comp,
+				usecs_to_jiffies(VSYNC_TIMEOUT_US));
+			cdm = mdss_mdp_cdm_alloc(mdata);
+		}
+	}
+
 	if (!cdm) {
 		pr_err("%s: Unable to allocate cdm\n", __func__);
 		return ERR_PTR(-EBUSY);
@@ -350,17 +375,6 @@ int mdss_mdp_cdm_destroy(struct mdss_mdp_cdm *cdm)
 		return -EINVAL;
 	}
 
-	mdss_mdp_clk_ctrl(MDP_BLOCK_POWER_ON);
-	mutex_lock(&cdm->lock);
-	/* Disable HDMI packer */
-	writel_relaxed(0x0, cdm->base + MDSS_MDP_REG_CDM_HDMI_PACK_OP_MODE);
-
-	/* Put CDM in bypass */
-	writel_relaxed(0x0, cdm->mdata->mdp_base + MDSS_MDP_MDP_OUT_CTL_0);
-
-	mutex_unlock(&cdm->lock);
-	mdss_mdp_clk_ctrl(MDP_BLOCK_POWER_OFF);
-
 	kref_put(&cdm->kref, mdss_mdp_cdm_free);
 
 	return rc;
diff --git a/drivers/video/fbdev/msm/mdss_mdp_cdm.h b/drivers/video/fbdev/msm/mdss_mdp_cdm.h
index 4515628da0e7..3b7fdced6623 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_cdm.h
+++ b/drivers/video/fbdev/msm/mdss_mdp_cdm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2014,2016, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -59,6 +59,7 @@ struct mdss_mdp_cdm {
 	u32 out_intf;
 	bool is_bypassed;
 	struct mdp_cdm_cfg setup;
+	struct completion free_comp;
 };
 
 struct mdss_mdp_cdm *mdss_mdp_cdm_init(struct mdss_mdp_ctl *ctl,
diff --git a/drivers/video/fbdev/msm/mdss_mdp_intf_video.c b/drivers/video/fbdev/msm/mdss_mdp_intf_video.c
index 048e5fce30c6..bf1e3617f18a 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_intf_video.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_intf_video.c
@@ -1793,19 +1793,10 @@ static inline bool mdss_mdp_video_need_pixel_drop(u32 vic)
 }
 
 static int mdss_mdp_video_cdm_setup(struct mdss_mdp_cdm *cdm,
-				   struct mdss_panel_info *pinfo)
+	struct mdss_panel_info *pinfo, struct mdss_mdp_format_params *fmt)
 {
-	struct mdss_mdp_format_params *fmt;
 	struct mdp_cdm_cfg setup;
 
-	fmt = mdss_mdp_get_format_params(pinfo->out_format);
-
-	if (!fmt) {
-		pr_err("%s: format %d not supported\n", __func__,
-		       pinfo->out_format);
-		return -EINVAL;
-	}
-	setup.out_format = pinfo->out_format;
 	if (fmt->is_yuv)
 		setup.csc_type = MDSS_MDP_CSC_RGB2YUV_601L;
 	else
@@ -1835,6 +1826,7 @@ static int mdss_mdp_video_cdm_setup(struct mdss_mdp_cdm *cdm,
 		return -EINVAL;
 	}
 
+	setup.out_format = pinfo->out_format;
 	setup.mdp_csc_bit_depth = MDP_CDM_CSC_8BIT;
 	setup.output_width = pinfo->xres + pinfo->lcdc.xres_pad;
 	setup.output_height = pinfo->yres + pinfo->lcdc.yres_pad;
@@ -1868,6 +1860,7 @@ static int mdss_mdp_video_ctx_setup(struct mdss_mdp_ctl *ctl,
 {
 	struct intf_timing_params *itp = &ctx->itp;
 	u32 dst_bpp;
+	struct mdss_mdp_format_params *fmt;
 	struct mdss_data_type *mdata = ctl->mdata;
 	struct dsc_desc *dsc = NULL;
 	u32 hdmi_dp_core;
@@ -1902,17 +1895,32 @@ static int mdss_mdp_video_ctx_setup(struct mdss_mdp_ctl *ctl,
 	}
 
 	if (mdss_mdp_is_cdm_supported(mdata, ctl->intf_type, 0)) {
-		ctl->cdm = mdss_mdp_cdm_init(ctl, MDP_CDM_CDWN_OUTPUT_HDMI);
-		if (!IS_ERR_OR_NULL(ctl->cdm)) {
-			if (mdss_mdp_video_cdm_setup(ctl->cdm, pinfo)) {
-				pr_err("%s: setting up cdm failed\n",
-				       __func__);
+
+		fmt = mdss_mdp_get_format_params(pinfo->out_format);
+		if (!fmt) {
+			pr_err("%s: format %d not supported\n", __func__,
+			       pinfo->out_format);
+			return -EINVAL;
+		}
+		if (fmt->is_yuv) {
+			ctl->cdm =
+			mdss_mdp_cdm_init(ctl, MDP_CDM_CDWN_OUTPUT_HDMI);
+			if (!IS_ERR_OR_NULL(ctl->cdm)) {
+				if (mdss_mdp_video_cdm_setup(ctl->cdm,
+					pinfo, fmt)) {
+					pr_err("%s: setting up cdm failed\n",
+					       __func__);
+					return -EINVAL;
+				}
+				ctl->flush_bits |= BIT(26);
+			} else {
+				pr_err("%s: failed to initialize cdm\n",
+					__func__);
 				return -EINVAL;
 			}
-			ctl->flush_bits |= BIT(26);
 		} else {
-			pr_err("%s: failed to initialize cdm\n", __func__);
-			return -EINVAL;
+			pr_debug("%s: Format is not YUV,cdm not required\n",
+				 __func__);
 		}
 	} else {
 		pr_debug("%s: cdm not supported\n", __func__);
diff --git a/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c b/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
index e6e03e7d54b2..11406e036534 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
@@ -202,16 +202,14 @@ static int mdss_mdp_writeback_addr_setup(struct mdss_mdp_writeback_ctx *ctx,
 }
 
 static int mdss_mdp_writeback_cdm_setup(struct mdss_mdp_writeback_ctx *ctx,
-					struct mdss_mdp_cdm *cdm, u32 format)
+	struct mdss_mdp_cdm *cdm, struct mdss_mdp_format_params *fmt)
 {
-	struct mdss_mdp_format_params *fmt;
 	struct mdp_cdm_cfg setup;
 
-	fmt = mdss_mdp_get_format_params(format);
-	if (!fmt) {
-		pr_err("%s: format %d not supported\n", __func__, format);
-		return -EINVAL;
-	}
+	if (fmt->is_yuv)
+		setup.csc_type = MDSS_MDP_CSC_RGB2YUV_601L;
+	else
+		setup.csc_type = MDSS_MDP_CSC_RGB2RGB;
 
 	if (fmt->is_yuv)
 		setup.csc_type = MDSS_MDP_CSC_RGB2YUV_601L;
@@ -237,7 +235,7 @@ static int mdss_mdp_writeback_cdm_setup(struct mdss_mdp_writeback_ctx *ctx,
 		return -EINVAL;
 	}
 
-	setup.out_format = format;
+	setup.out_format = fmt->format;
 	setup.mdp_csc_bit_depth = MDP_CDM_CSC_8BIT;
 	setup.output_width = ctx->width;
 	setup.output_height = ctx->height;
@@ -353,10 +351,9 @@ static int mdss_mdp_writeback_format_setup(struct mdss_mdp_writeback_ctx *ctx,
 	chroma_samp = fmt->chroma_sample;
 
 	if (ctl->cdm) {
-
-		rc = mdss_mdp_writeback_cdm_setup(ctx, ctl->cdm, format);
+		rc = mdss_mdp_writeback_cdm_setup(ctx, ctl->cdm, fmt);
 		if (rc) {
-			pr_err("%s: CDM configuration failed with error %d\n",
+			pr_err("%s: CDM config failed with error %d\n",
 				__func__, rc);
 			return rc;
 		}
@@ -1014,6 +1011,7 @@ int mdss_mdp_writeback_start(struct mdss_mdp_ctl *ctl)
 	struct mdss_mdp_writeback_ctx *ctx;
 	struct mdss_mdp_writeback *wb;
 	u32 mixer_type = MDSS_MDP_MIXER_TYPE_UNUSED;
+	struct mdss_mdp_format_params *fmt = NULL;
 	bool is_rot;
 
 	pr_debug("start ctl=%d\n", ctl->num);
@@ -1037,6 +1035,10 @@ int mdss_mdp_writeback_start(struct mdss_mdp_ctl *ctl)
 		return -EINVAL;
 	}
 
+	fmt = mdss_mdp_get_format_params(ctl->dst_format);
+	if (!fmt)
+		return -EINVAL;
+
 	is_rot = (ctx->type == MDSS_MDP_WRITEBACK_TYPE_ROTATOR) ? true : false;
 
 	if (ctl->mixer_left) {
@@ -1050,15 +1052,13 @@ int mdss_mdp_writeback_start(struct mdss_mdp_ctl *ctl)
 	}
 
 	if (mdss_mdp_is_cdm_supported(ctl->mdata, ctl->intf_type,
-				mixer_type)) {
+		mixer_type) && fmt->is_yuv) {
 		ctl->cdm = mdss_mdp_cdm_init(ctl, MDP_CDM_CDWN_OUTPUT_WB);
 		if (IS_ERR_OR_NULL(ctl->cdm)) {
-			pr_err("%s failed to init cdm\n", __func__);
+			pr_err("cdm block already in use\n");
+			ctl->cdm = NULL;
 			return -EBUSY;
 		}
-	} else {
-		ctl->cdm = NULL;
-		pr_debug("%s: cdm not supported\n", __func__);
 	}
 	ctl->priv_data = ctx;
 	ctx->wb_num = wb->num;