Merge "msm: mdss: fix mdp vbif writeback qos settings"

1 files changed, 48 insertions, 3 deletions

diff --git a/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c b/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
index e1d2a947a77f..5b284e624c7f 100644
--- a/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
+++ b/drivers/video/fbdev/msm/mdss_mdp_intf_writeback.c
@@ -125,6 +125,48 @@ static inline void mdp_wb_write(struct mdss_mdp_writeback_ctx *ctx,
 	writel_relaxed(val, ctx->base + reg);
 }
 
+static void mdss_mdp_qos_vbif_remapper_setup_wb(struct mdss_mdp_ctl *ctl,
+			struct mdss_mdp_writeback_ctx *ctx)
+{
+	u32 mask, reg_val, reg_val_lvl, reg_high, i, vbif_qos;
+	struct mdss_data_type *mdata = mdss_mdp_get_mdata();
+	bool is_nrt_vbif = (ctl->mixer_left && ctl->mixer_left->rotator_mode);
+
+	if (!mdata->vbif_nrt_qos)
+		return;
+
+	if (test_bit(MDSS_QOS_REMAPPER, mdata->mdss_qos_map)) {
+		mutex_lock(&mdata->reg_lock);
+		for (i = 0; i < mdata->npriority_lvl; i++) {
+			reg_high = ((ctx->xin_id & 0x8) >> 3) * 4 + (i * 8);
+
+			reg_val = MDSS_VBIF_READ(mdata,
+				MDSS_VBIF_QOS_RP_REMAP_BASE +
+				reg_high, is_nrt_vbif);
+			reg_val_lvl = MDSS_VBIF_READ(mdata,
+				MDSS_VBIF_QOS_LVL_REMAP_BASE + reg_high,
+				is_nrt_vbif);
+
+			mask = 0x3 << (ctx->xin_id * 4);
+			vbif_qos =  mdata->vbif_nrt_qos[i];
+
+			reg_val &= ~(mask);
+			reg_val |= vbif_qos << (ctx->xin_id * 4);
+
+			reg_val_lvl &= ~(mask);
+			reg_val_lvl |= vbif_qos << (ctx->xin_id * 4);
+
+			pr_debug("idx:%d xin:%d reg:0x%x val:0x%x lvl:0x%x\n",
+			   i, ctx->xin_id, reg_high, reg_val, reg_val_lvl);
+			MDSS_VBIF_WRITE(mdata, MDSS_VBIF_QOS_RP_REMAP_BASE +
+				reg_high, reg_val, is_nrt_vbif);
+			MDSS_VBIF_WRITE(mdata, MDSS_VBIF_QOS_LVL_REMAP_BASE +
+				reg_high, reg_val_lvl, is_nrt_vbif);
+		}
+		mutex_unlock(&mdata->reg_lock);
+	}
+}
+
 static void mdss_mdp_set_qos_wb(struct mdss_mdp_ctl *ctl,
 	struct mdss_mdp_writeback_ctx *ctx)
 {
@@ -133,12 +175,15 @@ static void mdss_mdp_set_qos_wb(struct mdss_mdp_ctl *ctl,
 	struct mdss_overlay_private *mdp5_data;
 	struct mdss_data_type *mdata = mdss_mdp_get_mdata();
 
-	if (false == test_bit(MDSS_QOS_WB_QOS, mdata->mdss_qos_map))
-		return;
-
 	mdp5_data = mfd_to_mdp5_data(ctl->mfd);
 	cwb = &mdp5_data->cwb;
 
+	if (!cwb->valid)
+		mdss_mdp_qos_vbif_remapper_setup_wb(ctl, ctx);
+
+	if (false == test_bit(MDSS_QOS_WB_QOS, mdata->mdss_qos_map))
+		return;
+
 	if (cwb->valid)
 		wb_qos_setup = QOS_LUT_CWB_READ;
 	else