msm: mdss: Add dither support for Thulium

MDP block supports dither feature in DSPPs (destination surface
processing pipes) which can be enabled or disabled by driver clients.
This change adds the support in post-processing driver to allow
dither configuration in DSPPs.

Change-Id: I2bd2da482f3d204649351bebdbe66bd5dab7a187
Signed-off-by: Ping Li <pingli@codeaurora.org>

drivers/video/fbdev/msm/mdss_mdp_pp.c

@@ -279,8 +279,6 @@ static int mdss_mdp_hscl_filter[] = {
 #define SHARP_SMOOTH_THR_DEFAULT	8
 #define SHARP_NOISE_THR_DEFAULT	2
 
-
-
 static struct mdp_pp_driver_ops pp_driver_ops;
 static struct mdp_pp_feature_ops *pp_ops;
 
@@ -1760,9 +1758,14 @@ static int pp_dspp_setup(u32 disp_num, struct mdss_mdp_mixer *mixer)
 		writel_relaxed(0, addr + 12);
 	}
 	if (flags & PP_FLAGS_DIRTY_DITHER) {
-		addr = base + MDSS_MDP_REG_DSPP_DITHER_DEPTH;
-		pp_dither_config(addr, pp_sts,
+		if (!pp_ops[DITHER].pp_set_config) {
+			pp_dither_config(addr, pp_sts,
 				&mdss_pp_res->dither_disp_cfg[disp_num]);
+		} else {
+			addr = base + MDSS_MDP_REG_DSPP_DITHER_DEPTH;
+			pp_ops[DITHER].pp_set_config(addr, pp_sts,
+			      &mdss_pp_res->dither_disp_cfg[disp_num], 0);
+		}
 	}
 	if (flags & PP_FLAGS_DIRTY_GAMUT) {
 		if (!pp_ops[GAMUT].pp_set_config) {
@@ -3449,12 +3452,15 @@ int mdss_mdp_dither_config(struct mdp_dither_cfg_data *config,
 					u32 *copyback)
 {
 	u32 disp_num;
+	int ret = 0;
 
 	if ((config->block < MDP_LOGICAL_BLOCK_DISP_0) ||
 		(config->block >= MDP_BLOCK_MAX))
 		return -EINVAL;
-	if (config->flags & MDP_PP_OPS_READ)
+	if (config->flags & MDP_PP_OPS_READ) {
+		pr_err("Dither read is not supported\n");
 		return -ENOTSUPP;
+	}
 
 	if ((config->flags & MDSS_PP_SPLIT_MASK) == MDSS_PP_SPLIT_MASK) {
 		pr_warn("Can't set both split bits\n");
@@ -3463,10 +3469,24 @@ int mdss_mdp_dither_config(struct mdp_dither_cfg_data *config,
 
 	mutex_lock(&mdss_pp_mutex);
 	disp_num = config->block - MDP_LOGICAL_BLOCK_DISP_0;
+	if (pp_ops[DITHER].pp_set_config) {
+		pr_debug("version of dither is %d\n", config->version);
+		ret = pp_dither_cache_params(config, mdss_pp_res);
+		if (ret) {
+			pr_err("dither config failed version %d ret %d\n",
+				config->version, ret);
+			goto dither_config_exit;
+		} else {
+			goto dither_set_dirty;
+		}
+	}
+
 	mdss_pp_res->dither_disp_cfg[disp_num] = *config;
+dither_set_dirty:
 	mdss_pp_res->pp_disp_flags[disp_num] |= PP_FLAGS_DIRTY_DITHER;
+dither_config_exit:
 	mutex_unlock(&mdss_pp_mutex);
-	return 0;
+	return ret;
 }
 
 static int pp_gm_has_invalid_lut_size(struct mdp_gamut_cfg_data *config)

drivers/video/fbdev/msm/mdss_mdp_pp.h

@@ -106,6 +106,7 @@ struct mdss_pp_res_type_v1_7 {
 	struct mdp_pgc_lut_data_v1_7 pgc_lm_v17_data[MDSS_BLOCK_LM_NUM];
 	struct mdp_igc_lut_data_v1_7 igc_v17_data[MDSS_BLOCK_DISP_NUM];
 	struct mdp_hist_lut_data_v1_7 hist_lut_v17_data[MDSS_BLOCK_DISP_NUM];
+	struct mdp_dither_data_v1_7 dither_v17_data[MDSS_BLOCK_DISP_NUM];
 	struct mdp_gamut_data_v1_7 gamut_v17_data[MDSS_BLOCK_DISP_NUM];
 	struct mdp_pcc_data_v1_7 pcc_v17_data[MDSS_BLOCK_DISP_NUM];
 };

drivers/video/fbdev/msm/mdss_mdp_pp_cache_config.c

@@ -106,6 +106,88 @@ int pp_hist_lut_cache_params(struct mdp_hist_lut_data *config,
 	return ret;
 }
 
+int pp_dither_cache_params_v1_7(struct mdp_dither_cfg_data *config,
+			  struct mdss_pp_res_type *mdss_pp_res)
+{
+	u32 disp_num;
+	int ret = 0;
+	struct mdss_pp_res_type_v1_7 *res_cache = NULL;
+	struct mdp_dither_data_v1_7 *v17_cache_data = NULL, v17_usr_config;
+
+	if (!config || !mdss_pp_res) {
+		pr_err("invalid param config %p pp_res %p\n",
+			config, mdss_pp_res);
+		return -EINVAL;
+	}
+	if ((config->block < MDP_LOGICAL_BLOCK_DISP_0) ||
+		(config->block >= MDP_BLOCK_MAX)) {
+		pr_err("invalid config block %d\n", config->block);
+		return -EINVAL;
+	}
+	if (!mdss_pp_res->pp_data_res) {
+		pr_err("invalid pp_data_res %p\n", mdss_pp_res->pp_data_res);
+		return -EINVAL;
+	}
+
+	res_cache = mdss_pp_res->pp_data_res;
+
+	if ((config->flags & MDSS_PP_SPLIT_MASK) == MDSS_PP_SPLIT_MASK) {
+		pr_warn("Can't set both split bits\n");
+		return -EINVAL;
+	}
+
+	if (config->flags & MDP_PP_OPS_READ) {
+		pr_err("read op is not supported\n");
+		return -EINVAL;
+	} else {
+		disp_num = config->block - MDP_LOGICAL_BLOCK_DISP_0;
+		mdss_pp_res->dither_disp_cfg[disp_num] = *config;
+		v17_cache_data = &res_cache->dither_v17_data[disp_num];
+		mdss_pp_res->dither_disp_cfg[disp_num].cfg_payload =
+			(void *)v17_cache_data;
+		if (copy_from_user(&v17_usr_config, config->cfg_payload,
+			sizeof(v17_usr_config))) {
+			pr_err("failed to copy v17 dither\n");
+			ret = -EFAULT;
+			goto dither_config_exit;
+		}
+		if ((config->flags & MDP_PP_OPS_DISABLE)) {
+			pr_debug("disable dither");
+			ret = 0;
+			goto dither_config_exit;
+		}
+		if (!(config->flags & MDP_PP_OPS_WRITE)) {
+			pr_debug("op for dither %d\n", config->flags);
+			goto dither_config_exit;
+		}
+		memcpy(v17_cache_data, &v17_usr_config, sizeof(v17_usr_config));
+	}
+dither_config_exit:
+	return ret;
+}
+
+int pp_dither_cache_params(struct mdp_dither_cfg_data *config,
+	struct mdss_pp_res_type *mdss_pp_res)
+{
+	int ret = 0;
+	if (!config || !mdss_pp_res) {
+		pr_err("invalid param config %pi pp_res %p\n",
+			config, mdss_pp_res);
+		return -EINVAL;
+	}
+	switch (config->version) {
+	case mdp_dither_v1_7:
+		ret = pp_dither_cache_params_v1_7(config, mdss_pp_res);
+		break;
+	default:
+		pr_err("unsupported dither version %d\n",
+			config->version);
+		break;
+	}
+	return ret;
+}
+
+
 static int pp_gamut_cache_params_v1_7(struct mdp_gamut_cfg_data *config,
 				      struct mdss_pp_res_type *mdss_pp_res)
 {
@@ -479,7 +561,6 @@ int pp_igc_lut_cache_params(struct mdp_igc_lut_data *config,
 	return ret;
 }
 
-
 static int pp_pgc_lut_cache_params_v1_7(struct mdp_pgc_lut_data *config,
 			    struct mdss_pp_res_type *mdss_pp_res,
 			    int location, int cnt)

drivers/video/fbdev/msm/mdss_mdp_pp_cache_config.h

@@ -19,6 +19,9 @@
 int pp_hist_lut_cache_params(struct mdp_hist_lut_data *config,
 			  struct mdss_pp_res_type *mdss_pp_res);
 
+int pp_dither_cache_params(struct mdp_dither_cfg_data *config,
+			  struct mdss_pp_res_type *mdss_pp_res);
+
 int pp_gamut_cache_params(struct mdp_gamut_cfg_data *config,
 			  struct mdss_pp_res_type *mdss_pp_res);
 int pp_pcc_cache_params(struct mdp_pcc_cfg_data *config,

drivers/video/fbdev/msm/mdss_mdp_pp_v1_7.c

@@ -83,6 +83,14 @@
 #define PGC_8B_ROUND BIT(1)
 #define PGC_ENABLE BIT(0)
 
+#define DITHER_MATRIX_OFF 0x14
+#define DITHER_MATRIX_INDEX 16
+#define DITHER_DEPTH_MAP_INDEX 9
+static u32 dither_matrix[DITHER_MATRIX_INDEX] = {
+	15, 7, 13, 5, 3, 11, 1, 9, 12, 4, 14, 6, 0, 8, 2, 10};
+static u32 dither_depth_map[DITHER_DEPTH_MAP_INDEX] = {
+	0, 0, 0, 0, 0, 1, 2, 3, 3};
+
 static struct mdss_pp_res_type_v1_7 config_data;
 
 static int pp_hist_lut_get_config(char __iomem *base_addr, void *cfg_data,
@@ -90,6 +98,11 @@ static int pp_hist_lut_get_config(char __iomem *base_addr, void *cfg_data,
 static int pp_hist_lut_set_config(char __iomem *base_addr,
 		struct pp_sts_type *pp_sts, void *cfg_data,
 		u32 block_type);
+static int pp_dither_get_config(char __iomem *base_addr, void *cfg_data,
+			   u32 block_type, u32 disp_num);
+static int pp_dither_set_config(char __iomem *base_addr,
+		struct pp_sts_type *pp_sts, void *cfg_data,
+		u32 block_type);
 
 static void pp_opmode_config(int location, struct pp_sts_type *pp_sts,
 		u32 *opmode, int side);
@@ -150,8 +163,8 @@ void *pp_get_driver_ops(struct mdp_pp_driver_ops *ops)
 	ops->pp_ops[CSC].pp_get_config = NULL;
 
 	/* Dither ops */
-	ops->pp_ops[DITHER].pp_set_config = NULL;
-	ops->pp_ops[DITHER].pp_get_config = NULL;
+	ops->pp_ops[DITHER].pp_set_config = pp_dither_set_config;
+	ops->pp_ops[DITHER].pp_get_config = pp_dither_get_config;
 
 	/* QSEED ops */
 	ops->pp_ops[QSEED].pp_set_config = NULL;
@@ -190,7 +203,8 @@ static void pp_opmode_config(int location, struct pp_sts_type *pp_sts,
 			if (pp_sts->enhist_sts & PP_STS_PA_LUT_FIRST)
 				*opmode |= MDSS_MDP_DSPP_OP_PA_LUTV_FIRST_EN;
 		}
-
+		if (pp_sts_is_enabled(pp_sts->dither_sts, side))
+			*opmode |= MDSS_MDP_DSPP_OP_DST_DITHER_EN;
 		break;
 	case LM:
 		if (pp_sts->argc_sts & PP_STS_ENABLE)
@@ -359,6 +373,83 @@ bail_out:
 	return ret;
 }
 
+static int pp_dither_get_config(char __iomem *base_addr, void *cfg_data,
+			   u32 block_type, u32 disp_num)
+{
+	pr_err("Operation not supported\n");
+	return -ENOTSUPP;
+}
+
+static int pp_dither_set_config(char __iomem *base_addr,
+		struct pp_sts_type *pp_sts, void *cfg_data,
+		u32 block_type)
+{
+	int i = 0;
+	u32 data;
+	struct mdp_dither_cfg_data *dither_cfg_data = NULL;
+	struct mdp_dither_data_v1_7 *dither_data = NULL;
+
+	if (!base_addr || !cfg_data || !pp_sts) {
+		pr_err("invalid params base_addr %p cfg_data %p pp_sts_type %p\n",
+		      base_addr, cfg_data, pp_sts);
+		return -EINVAL;
+	}
+
+	dither_cfg_data = (struct mdp_dither_cfg_data *) cfg_data;
+
+	if (dither_cfg_data->version != mdp_dither_v1_7 ||
+	    !dither_cfg_data->cfg_payload) {
+		pr_err("invalid dither version %d payload %p\n",
+		       dither_cfg_data->version, dither_cfg_data->cfg_payload);
+		return -EINVAL;
+	}
+	if (!(dither_cfg_data->flags & ~(MDP_PP_OPS_READ))) {
+		pr_err("only read ops set for lut\n");
+		return -EINVAL;
+	}
+	if (!(dither_cfg_data->flags & MDP_PP_OPS_WRITE)) {
+		pr_debug("non write ops set %d\n", dither_cfg_data->flags);
+		goto bail_out;
+	}
+
+	dither_data = dither_cfg_data->cfg_payload;
+	if (!dither_data) {
+		pr_err("invalid payload for dither %p\n", dither_data);
+		return -EINVAL;
+	}
+
+	if ((dither_data->g_y_depth >= DITHER_DEPTH_MAP_INDEX) ||
+		(dither_data->b_cb_depth >= DITHER_DEPTH_MAP_INDEX) ||
+		(dither_data->r_cr_depth >= DITHER_DEPTH_MAP_INDEX)) {
+		pr_err("invalid data for dither, g_y_depth %d y_cb_depth %d r_cr_depth %d\n",
+			dither_data->g_y_depth, dither_data->b_cb_depth,
+			dither_data->r_cr_depth);
+		return -EINVAL;
+	}
+	data = dither_depth_map[dither_data->g_y_depth];
+	data |= dither_depth_map[dither_data->b_cb_depth] << 2;
+	data |= dither_depth_map[dither_data->r_cr_depth] << 4;
+	data |= dither_cfg_data->mode << 8;
+	writel_relaxed(data, base_addr);
+	base_addr += DITHER_MATRIX_OFF;
+	for (i = 0; i < DITHER_MATRIX_INDEX; i += 4) {
+		data = dither_matrix[i] |
+			(dither_matrix[i + 1] << 4) |
+			(dither_matrix[i + 2] << 8) |
+			(dither_matrix[i + 3] << 12);
+		writel_relaxed(data, base_addr);
+		base_addr += 4;
+	}
+bail_out:
+	if (dither_cfg_data->flags & MDP_PP_OPS_DISABLE)
+		pp_sts->dither_sts &= ~PP_STS_ENABLE;
+	else if (dither_cfg_data->flags & MDP_PP_OPS_ENABLE)
+		pp_sts->dither_sts |= PP_STS_ENABLE;
+	pp_sts_set_split_bits(&pp_sts->dither_sts, dither_cfg_data->flags);
+
+	return 0;
+}
+
 static int pp_gamut_get_config(char __iomem *base_addr, void *cfg_data,
 			   u32 block_type, u32 disp_num)
 {

include/uapi/linux/msm_mdp.h

@@ -932,6 +932,11 @@ struct mdp_pa_cfg_data {
 	struct mdp_pa_cfg pa_data;
 };
 
+enum {
+	mdp_dither_v1_7 = 1,
+	mdp_dither_vmax,
+};
+
 struct mdp_dither_data_v1_7 {
 	uint32_t g_y_depth;
 	uint32_t r_cr_depth;