4 files changed, 59 insertions, 2 deletions

diff --git a/drivers/video/fbdev/msm/mdss_fb.c b/drivers/video/fbdev/msm/mdss_fb.c
index 2c26b3a8cb56..f5650d361b4f 100644
--- a/drivers/video/fbdev/msm/mdss_fb.c
+++ b/drivers/video/fbdev/msm/mdss_fb.c
@@ -1183,6 +1183,10 @@ static int mdss_fb_handle_pp_ioctl(void __user *argp)
 			break;
 		}
 		break;
+	case mdp_op_dither_cfg:
+		ret = mdss_mdp_dither_config(&mdp_pp.data.dither_cfg_data,
+				&copyback);
+		break;
 	default:
 		pr_err("Unsupported request to MDP_PP IOCTL.\n");
 		ret = -EINVAL;
diff --git a/drivers/video/fbdev/msm/mdss_mdp.h b/drivers/video/fbdev/msm/mdss_mdp.h
index 8d7b15282f51..71769e00ef70 100644
--- a/drivers/video/fbdev/msm/mdss_mdp.h
+++ b/drivers/video/fbdev/msm/mdss_mdp.h
@@ -306,6 +306,7 @@ int mdss_mdp_pcc_config(struct mdp_pcc_cfg_data *cfg_ptr, u32 *copyback);
 int mdss_mdp_igc_lut_config(struct mdp_igc_lut_data *config, u32 *copyback);
 int mdss_mdp_argc_config(struct mdp_pgc_lut_data *config, u32 *copyback);
 int mdss_mdp_hist_lut_config(struct mdp_hist_lut_data *config, u32 *copyback);
+int mdss_mdp_dither_config(struct mdp_dither_cfg_data *config, u32 *copyback);
 
 struct mdss_mdp_pipe *mdss_mdp_pipe_alloc_pnum(u32 pnum);
 struct mdss_mdp_pipe *mdss_mdp_pipe_alloc_locked(u32 type);
diff --git a/drivers/video/fbdev/msm/mdss_mdp_hwio.h b/drivers/video/fbdev/msm/mdss_mdp_hwio.h
index e757fa38717d..823ce88556bf 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_hwio.h
+++ b/drivers/video/fbdev/msm/mdss_mdp_hwio.h
@@ -333,6 +333,7 @@ enum mdss_mdp_dspp_index {
 #define MDSS_MDP_REG_DSPP_OFFSET(pipe)	(0x4600 + ((pipe) * 0x400))
 #define MDSS_MDP_REG_DSPP_OP_MODE			0x000
 #define MDSS_MDP_REG_DSPP_PCC_BASE			0x030
+#define MDSS_MDP_REG_DSPP_DITHER_DEPTH			0x150
 #define MDSS_MDP_REG_DSPP_PA_BASE			0x238
 #define MDSS_MDP_REG_DSPP_HIST_LUT_BASE			0x230
 
diff --git a/drivers/video/fbdev/msm/mdss_mdp_pp.c b/drivers/video/fbdev/msm/mdss_mdp_pp.c
index b76533c718f5..934234ee5f45 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_pp.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_pp.c
@@ -78,6 +78,12 @@ struct mdp_csc_cfg mdp_csc_convert[MDSS_MDP_MAX_CSC] = {
 #define GC_LUT_SEGMENTS	16
 #define ENHIST_LUT_ENTRIES 256
 
+static u32 dither_matrix[16] = {
+	15, 7, 13, 5, 3, 11, 1, 9, 12, 4, 14, 6, 0, 8, 2, 10};
+static u32 dither_depth_map[9] = {
+	0, 0, 0, 0, 0, 1, 2, 3, 3};
+
+
 struct pp_sts_type {
 	u32 pa_sts;
 	u32 pcc_sts;
@@ -85,6 +91,7 @@ struct pp_sts_type {
 	u32 igc_tbl_idx;
 	u32 argc_sts;
 	u32 enhist_sts;
+	u32 dither_sts;
 };
 
 #define PP_FLAGS_DIRTY_PA	0x1
@@ -92,6 +99,7 @@ struct pp_sts_type {
 #define PP_FLAGS_DIRTY_IGC	0x4
 #define PP_FLAGS_DIRTY_ARGC	0x8
 #define PP_FLAGS_DIRTY_ENHIST	0x10
+#define PP_FLAGS_DIRTY_DITHER	0x20
 
 #define PP_STS_ENABLE	0x1
 
@@ -112,6 +120,7 @@ struct mdss_pp_res_type {
 	struct mdp_igc_lut_data igc_disp_cfg[MDSS_BLOCK_DISP_NUM];
 	struct mdp_pgc_lut_data pgc_disp_cfg[MDSS_BLOCK_DISP_NUM];
 	struct mdp_hist_lut_data enhist_disp_cfg[MDSS_BLOCK_DISP_NUM];
+	struct mdp_dither_cfg_data dither_disp_cfg[MDSS_BLOCK_DISP_NUM];
 	/* physical info */
 	struct pp_sts_type pp_dspp_sts[MDSS_MDP_MAX_DSPP];
 };
@@ -266,8 +275,9 @@ static int pp_dspp_setup(u32 disp_num, struct mdss_mdp_ctl *ctl,
 	struct mdp_pcc_cfg_data *pcc_config;
 	struct mdp_igc_lut_data *igc_config;
 	struct mdp_hist_lut_data *enhist_cfg;
+	struct mdp_dither_cfg_data *dither_cfg;
 	struct pp_sts_type *pp_sts;
-	u32 tbl_idx;
+	u32 data, tbl_idx;
 	int i;
 	dspp_num = mixer->num;
 	/* no corresponding dspp */
@@ -369,7 +379,31 @@ static int pp_dspp_setup(u32 disp_num, struct mdss_mdp_ctl *ctl,
 			MDSS_MDP_REG_WRITE(offset + 12, 0);
 		}
 	}
-
+	if (flags & PP_FLAGS_DIRTY_DITHER) {
+		dither_cfg = &mdss_pp_res->dither_disp_cfg[disp_num];
+		if (dither_cfg->flags & MDP_PP_OPS_WRITE) {
+			offset = base + MDSS_MDP_REG_DSPP_DITHER_DEPTH;
+			MDSS_MDP_REG_WRITE(offset,
+			  dither_depth_map[dither_cfg->g_y_depth] |
+			  (dither_depth_map[dither_cfg->b_cb_depth] << 2) |
+			  (dither_depth_map[dither_cfg->r_cr_depth] << 4));
+			offset += 0x14;
+			for (i = 0; i << 16; i += 4) {
+				data = dither_matrix[i] |
+					(dither_matrix[i + 1] << 4) |
+					(dither_matrix[i + 2] << 8) |
+					(dither_matrix[i + 3] << 12);
+				MDSS_MDP_REG_WRITE(offset, data);
+				offset += 4;
+			}
+		}
+		if (dither_cfg->flags & MDP_PP_OPS_DISABLE)
+			pp_sts->dither_sts &= ~PP_STS_ENABLE;
+		else if (dither_cfg->flags & MDP_PP_OPS_ENABLE)
+			pp_sts->dither_sts |= PP_STS_ENABLE;
+	}
+	if (pp_sts->dither_sts & PP_STS_ENABLE)
+		opmode |= (1 << 8); /* DITHER_EN */
 	MDSS_MDP_REG_WRITE(base + MDSS_MDP_REG_DSPP_OP_MODE, opmode);
 	ctl->flush_bits |= BIT(13 + dspp_num); /* DSPP */
 	return 0;
@@ -988,3 +1022,20 @@ enhist_config_exit:
 	mutex_unlock(&mdss_pp_mutex);
 	return ret;
 }
+
+int mdss_mdp_dither_config(struct mdp_dither_cfg_data *config, u32 *copyback)
+{
+	u32 disp_num;
+	if ((config->block < MDP_LOGICAL_BLOCK_DISP_0) ||
+		(config->block >= MDP_BLOCK_MAX))
+		return -EINVAL;
+	if (config->flags & MDP_PP_OPS_READ)
+		return -ENOTSUPP;
+
+	mutex_lock(&mdss_pp_mutex);
+	disp_num = config->block - MDP_LOGICAL_BLOCK_DISP_0;
+	mdss_pp_res->dither_disp_cfg[disp_num] = *config;
+	mdss_pp_res->pp_disp_flags[disp_num] |= PP_FLAGS_DIRTY_DITHER;
+	mutex_unlock(&mdss_pp_mutex);
+	return 0;
+}